      SUBROUTINE ZGELSS( M, N, NRHS, A, LDA, B, LDB, S, RCOND, RANK,
     $                   WORK, LWORK, RWORK, INFO )
*
*  -- LAPACK driver routine (version 3.2) --
*  -- LAPACK is a software package provided by Univ. of Tennessee,    --
*  -- Univ. of California Berkeley, Univ. of Colorado Denver and NAG Ltd..--
*     November 2006
*
*     .. Scalar Arguments ..
      INTEGER            INFO, LDA, LDB, LWORK, M, N, NRHS, RANK
      DOUBLE PRECISION   RCOND
*     ..
*     .. Array Arguments ..
      DOUBLE PRECISION   RWORK( * ), S( * )
      COMPLEX*16         A( LDA, * ), B( LDB, * ), WORK( * )
*     ..
*
*  Purpose
*  =======
*
*  ZGELSS computes the minimum norm solution to a complex linear
*  least squares problem:
*
*  Minimize 2-norm(| b - A*x |).
*
*  using the singular value decomposition (SVD) of A. A is an M-by-N
*  matrix which may be rank-deficient.
*
*  Several right hand side vectors b and solution vectors x can be
*  handled in a single call; they are stored as the columns of the
*  M-by-NRHS right hand side matrix B and the N-by-NRHS solution matrix
*  X.
*
*  The effective rank of A is determined by treating as zero those
*  singular values which are less than RCOND times the largest singular
*  value.
*
*  Arguments
*  =========
*
*  M       (input) INTEGER
*          The number of rows of the matrix A. M >= 0.
*
*  N       (input) INTEGER
*          The number of columns of the matrix A. N >= 0.
*
*  NRHS    (input) INTEGER
*          The number of right hand sides, i.e., the number of columns
*          of the matrices B and X. NRHS >= 0.
*
*  A       (input/output) COMPLEX*16 array, dimension (LDA,N)
*          On entry, the M-by-N matrix A.
*          On exit, the first min(m,n) rows of A are overwritten with
*          its right singular vectors, stored rowwise.
*
*  LDA     (input) INTEGER
*          The leading dimension of the array A. LDA >= max(1,M).
*
*  B       (input/output) COMPLEX*16 array, dimension (LDB,NRHS)
*          On entry, the M-by-NRHS right hand side matrix B.
*          On exit, B is overwritten by the N-by-NRHS solution matrix X.
*          If m >= n and RANK = n, the residual sum-of-squares for
*          the solution in the i-th column is given by the sum of
*          squares of the modulus of elements n+1:m in that column.
*
*  LDB     (input) INTEGER
*          The leading dimension of the array B.  LDB >= max(1,M,N).
*
*  S       (output) DOUBLE PRECISION array, dimension (min(M,N))
*          The singular values of A in decreasing order.
*          The condition number of A in the 2-norm = S(1)/S(min(m,n)).
*
*  RCOND   (input) DOUBLE PRECISION
*          RCOND is used to determine the effective rank of A.
*          Singular values S(i) <= RCOND*S(1) are treated as zero.
*          If RCOND < 0, machine precision is used instead.
*
*  RANK    (output) INTEGER
*          The effective rank of A, i.e., the number of singular values
*          which are greater than RCOND*S(1).
*
*  WORK    (workspace/output) COMPLEX*16 array, dimension (MAX(1,LWORK))
*          On exit, if INFO = 0, WORK(1) returns the optimal LWORK.
*
*  LWORK   (input) INTEGER
*          The dimension of the array WORK. LWORK >= 1, and also:
*          LWORK >=  2*min(M,N) + max(M,N,NRHS)
*          For good performance, LWORK should generally be larger.
*
*          If LWORK = -1, then a workspace query is assumed; the routine
*          only calculates the optimal size of the WORK array, returns
*          this value as the first entry of the WORK array, and no error
*          message related to LWORK is issued by XERBLA.
*
*  RWORK   (workspace) DOUBLE PRECISION array, dimension (5*min(M,N))
*
*  INFO    (output) INTEGER
*          = 0:  successful exit
*          < 0:  if INFO = -i, the i-th argument had an illegal value.
*          > 0:  the algorithm for computing the SVD failed to converge;
*                if INFO = i, i off-diagonal elements of an intermediate
*                bidiagonal form did not converge to zero.
*
*  =====================================================================
*
*     .. Parameters ..
      DOUBLE PRECISION   ZERO, ONE
      PARAMETER          ( ZERO = 0.0D+0, ONE = 1.0D+0 )
      COMPLEX*16         CZERO, CONE
      PARAMETER          ( CZERO = ( 0.0D+0, 0.0D+0 ),
     $                   CONE = ( 1.0D+0, 0.0D+0 ) )
*     ..
*     .. Local Scalars ..
      LOGICAL            LQUERY
      INTEGER            BL, CHUNK, I, IASCL, IBSCL, IE, IL, IRWORK,
     $                   ITAU, ITAUP, ITAUQ, IWORK, LDWORK, MAXMN,
     $                   MAXWRK, MINMN, MINWRK, MM, MNTHR
      DOUBLE PRECISION   ANRM, BIGNUM, BNRM, EPS, SFMIN, SMLNUM, THR
*     ..
*     .. Local Arrays ..
      COMPLEX*16         VDUM( 1 )
*     ..
*     .. External Subroutines ..
      EXTERNAL           DLABAD, DLASCL, DLASET, XERBLA, ZBDSQR, ZCOPY,
     $                   ZDRSCL, ZGEBRD, ZGELQF, ZGEMM, ZGEMV, ZGEQRF,
     $                   ZLACPY, ZLASCL, ZLASET, ZUNGBR, ZUNMBR, ZUNMLQ,
     $                   ZUNMQR
*     ..
*     .. External Functions ..
      INTEGER            ILAENV
      DOUBLE PRECISION   DLAMCH, ZLANGE
      EXTERNAL           ILAENV, DLAMCH, ZLANGE
*     ..
*     .. Intrinsic Functions ..
      INTRINSIC          MAX, MIN
*     ..
*     .. Executable Statements ..
*
*     Test the input arguments
*
      INFO = 0
      MINMN = MIN( M, N )
      MAXMN = MAX( M, N )
      LQUERY = ( LWORK.EQ.-1 )
      IF( M.LT.0 ) THEN
         INFO = -1
      ELSE IF( N.LT.0 ) THEN
         INFO = -2
      ELSE IF( NRHS.LT.0 ) THEN
         INFO = -3
      ELSE IF( LDA.LT.MAX( 1, M ) ) THEN
         INFO = -5
      ELSE IF( LDB.LT.MAX( 1, MAXMN ) ) THEN
         INFO = -7
      END IF
*
*     Compute workspace
*      (Note: Comments in the code beginning "Workspace:" describe the
*       minimal amount of workspace needed at that point in the code,
*       as well as the preferred amount for good performance.
*       CWorkspace refers to complex workspace, and RWorkspace refers
*       to real workspace. NB refers to the optimal block size for the
*       immediately following subroutine, as returned by ILAENV.)
*
      IF( INFO.EQ.0 ) THEN
         MINWRK = 1
         MAXWRK = 1
         IF( MINMN.GT.0 ) THEN
            MM = M
            MNTHR = ILAENV( 6, 'ZGELSS', ' ', M, N, NRHS, -1 )
            IF( M.GE.N .AND. M.GE.MNTHR ) THEN
*
*              Path 1a - overdetermined, with many more rows than
*                        columns
*
               MM = N
               MAXWRK = MAX( MAXWRK, N + N*ILAENV( 1, 'ZGEQRF', ' ', M,
     $                       N, -1, -1 ) )
               MAXWRK = MAX( MAXWRK, N + NRHS*ILAENV( 1, 'ZUNMQR', 'LC',
     $                       M, NRHS, N, -1 ) )
            END IF
            IF( M.GE.N ) THEN
*
*              Path 1 - overdetermined or exactly determined
*
               MAXWRK = MAX( MAXWRK, 2*N + ( MM + N )*ILAENV( 1,
     $                       'ZGEBRD', ' ', MM, N, -1, -1 ) )
               MAXWRK = MAX( MAXWRK, 2*N + NRHS*ILAENV( 1, 'ZUNMBR',
     $                       'QLC', MM, NRHS, N, -1 ) )
               MAXWRK = MAX( MAXWRK, 2*N + ( N - 1 )*ILAENV( 1,
     $                       'ZUNGBR', 'P', N, N, N, -1 ) )
               MAXWRK = MAX( MAXWRK, N*NRHS )
               MINWRK = 2*N + MAX( NRHS, M )
            END IF
            IF( N.GT.M ) THEN
               MINWRK = 2*M + MAX( NRHS, N )
               IF( N.GE.MNTHR ) THEN
*
*                 Path 2a - underdetermined, with many more columns
*                 than rows
*
                  MAXWRK = M + M*ILAENV( 1, 'ZGELQF', ' ', M, N, -1,
     $                     -1 )
                  MAXWRK = MAX( MAXWRK, 3*M + M*M + 2*M*ILAENV( 1,
     $                          'ZGEBRD', ' ', M, M, -1, -1 ) )
                  MAXWRK = MAX( MAXWRK, 3*M + M*M + NRHS*ILAENV( 1,
     $                          'ZUNMBR', 'QLC', M, NRHS, M, -1 ) )
                  MAXWRK = MAX( MAXWRK, 3*M + M*M + ( M - 1 )*ILAENV( 1,
     $                          'ZUNGBR', 'P', M, M, M, -1 ) )
                  IF( NRHS.GT.1 ) THEN
                     MAXWRK = MAX( MAXWRK, M*M + M + M*NRHS )
                  ELSE
                     MAXWRK = MAX( MAXWRK, M*M + 2*M )
                  END IF
                  MAXWRK = MAX( MAXWRK, M + NRHS*ILAENV( 1, 'ZUNMLQ',
     $                          'LC', N, NRHS, M, -1 ) )
               ELSE
*
*                 Path 2 - underdetermined
*
                  MAXWRK = 2*M + ( N + M )*ILAENV( 1, 'ZGEBRD', ' ', M,
     $                     N, -1, -1 )
                  MAXWRK = MAX( MAXWRK, 2*M + NRHS*ILAENV( 1, 'ZUNMBR',
     $                          'QLC', M, NRHS, M, -1 ) )
                  MAXWRK = MAX( MAXWRK, 2*M + M*ILAENV( 1, 'ZUNGBR',
     $                          'P', M, N, M, -1 ) )
                  MAXWRK = MAX( MAXWRK, N*NRHS )
               END IF
            END IF
            MAXWRK = MAX( MINWRK, MAXWRK )
         END IF
         WORK( 1 ) = MAXWRK
*
         IF( LWORK.LT.MINWRK .AND. .NOT.LQUERY )
     $      INFO = -12
      END IF
*
      IF( INFO.NE.0 ) THEN
         CALL XERBLA( 'ZGELSS', -INFO )
         RETURN
      ELSE IF( LQUERY ) THEN
         RETURN
      END IF
*
*     Quick return if possible
*
      IF( M.EQ.0 .OR. N.EQ.0 ) THEN
         RANK = 0
         RETURN
      END IF
*
*     Get machine parameters
*
      EPS = DLAMCH( 'P' )
      SFMIN = DLAMCH( 'S' )
      SMLNUM = SFMIN / EPS
      BIGNUM = ONE / SMLNUM
      CALL DLABAD( SMLNUM, BIGNUM )
*
*     Scale A if max element outside range [SMLNUM,BIGNUM]
*
      ANRM = ZLANGE( 'M', M, N, A, LDA, RWORK )
      IASCL = 0
      IF( ANRM.GT.ZERO .AND. ANRM.LT.SMLNUM ) THEN
*
*        Scale matrix norm up to SMLNUM
*
         CALL ZLASCL( 'G', 0, 0, ANRM, SMLNUM, M, N, A, LDA, INFO )
         IASCL = 1
      ELSE IF( ANRM.GT.BIGNUM ) THEN
*
*        Scale matrix norm down to BIGNUM
*
         CALL ZLASCL( 'G', 0, 0, ANRM, BIGNUM, M, N, A, LDA, INFO )
         IASCL = 2
      ELSE IF( ANRM.EQ.ZERO ) THEN
*
*        Matrix all zero. Return zero solution.
*
         CALL ZLASET( 'F', MAX( M, N ), NRHS, CZERO, CZERO, B, LDB )
         CALL DLASET( 'F', MINMN, 1, ZERO, ZERO, S, MINMN )
         RANK = 0
         GO TO 70
      END IF
*
*     Scale B if max element outside range [SMLNUM,BIGNUM]
*
      BNRM = ZLANGE( 'M', M, NRHS, B, LDB, RWORK )
      IBSCL = 0
      IF( BNRM.GT.ZERO .AND. BNRM.LT.SMLNUM ) THEN
*
*        Scale matrix norm up to SMLNUM
*
         CALL ZLASCL( 'G', 0, 0, BNRM, SMLNUM, M, NRHS, B, LDB, INFO )
         IBSCL = 1
      ELSE IF( BNRM.GT.BIGNUM ) THEN
*
*        Scale matrix norm down to BIGNUM
*
         CALL ZLASCL( 'G', 0, 0, BNRM, BIGNUM, M, NRHS, B, LDB, INFO )
         IBSCL = 2
      END IF
*
*     Overdetermined case
*
      IF( M.GE.N ) THEN
*
*        Path 1 - overdetermined or exactly determined
*
         MM = M
         IF( M.GE.MNTHR ) THEN
*
*           Path 1a - overdetermined, with many more rows than columns
*
            MM = N
            ITAU = 1
            IWORK = ITAU + N
*
*           Compute A=Q*R
*           (CWorkspace: need 2*N, prefer N+N*NB)
*           (RWorkspace: none)
*
            CALL ZGEQRF( M, N, A, LDA, WORK( ITAU ), WORK( IWORK ),
     $                   LWORK-IWORK+1, INFO )
*
*           Multiply B by transpose(Q)
*           (CWorkspace: need N+NRHS, prefer N+NRHS*NB)
*           (RWorkspace: none)
*
            CALL ZUNMQR( 'L', 'C', M, NRHS, N, A, LDA, WORK( ITAU ), B,
     $                   LDB, WORK( IWORK ), LWORK-IWORK+1, INFO )
*
*           Zero out below R
*
            IF( N.GT.1 )
     $         CALL ZLASET( 'L', N-1, N-1, CZERO, CZERO, A( 2, 1 ),
     $                      LDA )
         END IF
*
         IE = 1
         ITAUQ = 1
         ITAUP = ITAUQ + N
         IWORK = ITAUP + N
*
*        Bidiagonalize R in A
*        (CWorkspace: need 2*N+MM, prefer 2*N+(MM+N)*NB)
*        (RWorkspace: need N)
*
         CALL ZGEBRD( MM, N, A, LDA, S, RWORK( IE ), WORK( ITAUQ ),
     $                WORK( ITAUP ), WORK( IWORK ), LWORK-IWORK+1,
     $                INFO )
*
*        Multiply B by transpose of left bidiagonalizing vectors of R
*        (CWorkspace: need 2*N+NRHS, prefer 2*N+NRHS*NB)
*        (RWorkspace: none)
*
         CALL ZUNMBR( 'Q', 'L', 'C', MM, NRHS, N, A, LDA, WORK( ITAUQ ),
     $                B, LDB, WORK( IWORK ), LWORK-IWORK+1, INFO )
*
*        Generate right bidiagonalizing vectors of R in A
*        (CWorkspace: need 3*N-1, prefer 2*N+(N-1)*NB)
*        (RWorkspace: none)
*
         CALL ZUNGBR( 'P', N, N, N, A, LDA, WORK( ITAUP ),
     $                WORK( IWORK ), LWORK-IWORK+1, INFO )
         IRWORK = IE + N
*
*        Perform bidiagonal QR iteration
*          multiply B by transpose of left singular vectors
*          compute right singular vectors in A
*        (CWorkspace: none)
*        (RWorkspace: need BDSPAC)
*
         CALL ZBDSQR( 'U', N, N, 0, NRHS, S, RWORK( IE ), A, LDA, VDUM,
     $                1, B, LDB, RWORK( IRWORK ), INFO )
         IF( INFO.NE.0 )
     $      GO TO 70
*
*        Multiply B by reciprocals of singular values
*
         THR = MAX( RCOND*S( 1 ), SFMIN )
         IF( RCOND.LT.ZERO )
     $      THR = MAX( EPS*S( 1 ), SFMIN )
         RANK = 0
         DO 10 I = 1, N
            IF( S( I ).GT.THR ) THEN
               CALL ZDRSCL( NRHS, S( I ), B( I, 1 ), LDB )
               RANK = RANK + 1
            ELSE
               CALL ZLASET( 'F', 1, NRHS, CZERO, CZERO, B( I, 1 ), LDB )
            END IF
   10    CONTINUE
*
*        Multiply B by right singular vectors
*        (CWorkspace: need N, prefer N*NRHS)
*        (RWorkspace: none)
*
         IF( LWORK.GE.LDB*NRHS .AND. NRHS.GT.1 ) THEN
            CALL ZGEMM( 'C', 'N', N, NRHS, N, CONE, A, LDA, B, LDB,
     $                  CZERO, WORK, LDB )
            CALL ZLACPY( 'G', N, NRHS, WORK, LDB, B, LDB )
         ELSE IF( NRHS.GT.1 ) THEN
            CHUNK = LWORK / N
            DO 20 I = 1, NRHS, CHUNK
               BL = MIN( NRHS-I+1, CHUNK )
               CALL ZGEMM( 'C', 'N', N, BL, N, CONE, A, LDA, B( 1, I ),
     $                     LDB, CZERO, WORK, N )
               CALL ZLACPY( 'G', N, BL, WORK, N, B( 1, I ), LDB )
   20       CONTINUE
         ELSE
            CALL ZGEMV( 'C', N, N, CONE, A, LDA, B, 1, CZERO, WORK, 1 )
            CALL ZCOPY( N, WORK, 1, B, 1 )
         END IF
*
      ELSE IF( N.GE.MNTHR .AND. LWORK.GE.3*M+M*M+MAX( M, NRHS, N-2*M ) )
     $          THEN
*
*        Underdetermined case, M much less than N
*
*        Path 2a - underdetermined, with many more columns than rows
*        and sufficient workspace for an efficient algorithm
*
         LDWORK = M
         IF( LWORK.GE.3*M+M*LDA+MAX( M, NRHS, N-2*M ) )
     $      LDWORK = LDA
         ITAU = 1
         IWORK = M + 1
*
*        Compute A=L*Q
*        (CWorkspace: need 2*M, prefer M+M*NB)
*        (RWorkspace: none)
*
         CALL ZGELQF( M, N, A, LDA, WORK( ITAU ), WORK( IWORK ),
     $                LWORK-IWORK+1, INFO )
         IL = IWORK
*
*        Copy L to WORK(IL), zeroing out above it
*
         CALL ZLACPY( 'L', M, M, A, LDA, WORK( IL ), LDWORK )
         CALL ZLASET( 'U', M-1, M-1, CZERO, CZERO, WORK( IL+LDWORK ),
     $                LDWORK )
         IE = 1
         ITAUQ = IL + LDWORK*M
         ITAUP = ITAUQ + M
         IWORK = ITAUP + M
*
*        Bidiagonalize L in WORK(IL)
*        (CWorkspace: need M*M+4*M, prefer M*M+3*M+2*M*NB)
*        (RWorkspace: need M)
*
         CALL ZGEBRD( M, M, WORK( IL ), LDWORK, S, RWORK( IE ),
     $                WORK( ITAUQ ), WORK( ITAUP ), WORK( IWORK ),
     $                LWORK-IWORK+1, INFO )
*
*        Multiply B by transpose of left bidiagonalizing vectors of L
*        (CWorkspace: need M*M+3*M+NRHS, prefer M*M+3*M+NRHS*NB)
*        (RWorkspace: none)
*
         CALL ZUNMBR( 'Q', 'L', 'C', M, NRHS, M, WORK( IL ), LDWORK,
     $                WORK( ITAUQ ), B, LDB, WORK( IWORK ),
     $                LWORK-IWORK+1, INFO )
*
*        Generate right bidiagonalizing vectors of R in WORK(IL)
*        (CWorkspace: need M*M+4*M-1, prefer M*M+3*M+(M-1)*NB)
*        (RWorkspace: none)
*
         CALL ZUNGBR( 'P', M, M, M, WORK( IL ), LDWORK, WORK( ITAUP ),
     $                WORK( IWORK ), LWORK-IWORK+1, INFO )
         IRWORK = IE + M
*
*        Perform bidiagonal QR iteration, computing right singular
*        vectors of L in WORK(IL) and multiplying B by transpose of
*        left singular vectors
*        (CWorkspace: need M*M)
*        (RWorkspace: need BDSPAC)
*
         CALL ZBDSQR( 'U', M, M, 0, NRHS, S, RWORK( IE ), WORK( IL ),
     $                LDWORK, A, LDA, B, LDB, RWORK( IRWORK ), INFO )
         IF( INFO.NE.0 )
     $      GO TO 70
*
*        Multiply B by reciprocals of singular values
*
         THR = MAX( RCOND*S( 1 ), SFMIN )
         IF( RCOND.LT.ZERO )
     $      THR = MAX( EPS*S( 1 ), SFMIN )
         RANK = 0
         DO 30 I = 1, M
            IF( S( I ).GT.THR ) THEN
               CALL ZDRSCL( NRHS, S( I ), B( I, 1 ), LDB )
               RANK = RANK + 1
            ELSE
               CALL ZLASET( 'F', 1, NRHS, CZERO, CZERO, B( I, 1 ), LDB )
            END IF
   30    CONTINUE
         IWORK = IL + M*LDWORK
*
*        Multiply B by right singular vectors of L in WORK(IL)
*        (CWorkspace: need M*M+2*M, prefer M*M+M+M*NRHS)
*        (RWorkspace: none)
*
         IF( LWORK.GE.LDB*NRHS+IWORK-1 .AND. NRHS.GT.1 ) THEN
            CALL ZGEMM( 'C', 'N', M, NRHS, M, CONE, WORK( IL ), LDWORK,
     $                  B, LDB, CZERO, WORK( IWORK ), LDB )
            CALL ZLACPY( 'G', M, NRHS, WORK( IWORK ), LDB, B, LDB )
         ELSE IF( NRHS.GT.1 ) THEN
            CHUNK = ( LWORK-IWORK+1 ) / M
            DO 40 I = 1, NRHS, CHUNK
               BL = MIN( NRHS-I+1, CHUNK )
               CALL ZGEMM( 'C', 'N', M, BL, M, CONE, WORK( IL ), LDWORK,
     $                     B( 1, I ), LDB, CZERO, WORK( IWORK ), M )
               CALL ZLACPY( 'G', M, BL, WORK( IWORK ), M, B( 1, I ),
     $                      LDB )
   40       CONTINUE
         ELSE
            CALL ZGEMV( 'C', M, M, CONE, WORK( IL ), LDWORK, B( 1, 1 ),
     $                  1, CZERO, WORK( IWORK ), 1 )
            CALL ZCOPY( M, WORK( IWORK ), 1, B( 1, 1 ), 1 )
         END IF
*
*        Zero out below first M rows of B
*
         CALL ZLASET( 'F', N-M, NRHS, CZERO, CZERO, B( M+1, 1 ), LDB )
         IWORK = ITAU + M
*
*        Multiply transpose(Q) by B
*        (CWorkspace: need M+NRHS, prefer M+NHRS*NB)
*        (RWorkspace: none)
*
         CALL ZUNMLQ( 'L', 'C', N, NRHS, M, A, LDA, WORK( ITAU ), B,
     $                LDB, WORK( IWORK ), LWORK-IWORK+1, INFO )
*
      ELSE
*
*        Path 2 - remaining underdetermined cases
*
         IE = 1
         ITAUQ = 1
         ITAUP = ITAUQ + M
         IWORK = ITAUP + M
*
*        Bidiagonalize A
*        (CWorkspace: need 3*M, prefer 2*M+(M+N)*NB)
*        (RWorkspace: need N)
*
         CALL ZGEBRD( M, N, A, LDA, S, RWORK( IE ), WORK( ITAUQ ),
     $                WORK( ITAUP ), WORK( IWORK ), LWORK-IWORK+1,
     $                INFO )
*
*        Multiply B by transpose of left bidiagonalizing vectors
*        (CWorkspace: need 2*M+NRHS, prefer 2*M+NRHS*NB)
*        (RWorkspace: none)
*
         CALL ZUNMBR( 'Q', 'L', 'C', M, NRHS, N, A, LDA, WORK( ITAUQ ),
     $                B, LDB, WORK( IWORK ), LWORK-IWORK+1, INFO )
*
*        Generate right bidiagonalizing vectors in A
*        (CWorkspace: need 3*M, prefer 2*M+M*NB)
*        (RWorkspace: none)
*
         CALL ZUNGBR( 'P', M, N, M, A, LDA, WORK( ITAUP ),
     $                WORK( IWORK ), LWORK-IWORK+1, INFO )
         IRWORK = IE + M
*
*        Perform bidiagonal QR iteration,
*           computing right singular vectors of A in A and
*           multiplying B by transpose of left singular vectors
*        (CWorkspace: none)
*        (RWorkspace: need BDSPAC)
*
         CALL ZBDSQR( 'L', M, N, 0, NRHS, S, RWORK( IE ), A, LDA, VDUM,
     $                1, B, LDB, RWORK( IRWORK ), INFO )
         IF( INFO.NE.0 )
     $      GO TO 70
*
*        Multiply B by reciprocals of singular values
*
         THR = MAX( RCOND*S( 1 ), SFMIN )
         IF( RCOND.LT.ZERO )
     $      THR = MAX( EPS*S( 1 ), SFMIN )
         RANK = 0
         DO 50 I = 1, M
            IF( S( I ).GT.THR ) THEN
               CALL ZDRSCL( NRHS, S( I ), B( I, 1 ), LDB )
               RANK = RANK + 1
            ELSE
               CALL ZLASET( 'F', 1, NRHS, CZERO, CZERO, B( I, 1 ), LDB )
            END IF
   50    CONTINUE
*
*        Multiply B by right singular vectors of A
*        (CWorkspace: need N, prefer N*NRHS)
*        (RWorkspace: none)
*
         IF( LWORK.GE.LDB*NRHS .AND. NRHS.GT.1 ) THEN
            CALL ZGEMM( 'C', 'N', N, NRHS, M, CONE, A, LDA, B, LDB,
     $                  CZERO, WORK, LDB )
            CALL ZLACPY( 'G', N, NRHS, WORK, LDB, B, LDB )
         ELSE IF( NRHS.GT.1 ) THEN
            CHUNK = LWORK / N
            DO 60 I = 1, NRHS, CHUNK
               BL = MIN( NRHS-I+1, CHUNK )
               CALL ZGEMM( 'C', 'N', N, BL, M, CONE, A, LDA, B( 1, I ),
     $                     LDB, CZERO, WORK, N )
               CALL ZLACPY( 'F', N, BL, WORK, N, B( 1, I ), LDB )
   60       CONTINUE
         ELSE
            CALL ZGEMV( 'C', M, N, CONE, A, LDA, B, 1, CZERO, WORK, 1 )
            CALL ZCOPY( N, WORK, 1, B, 1 )
         END IF
      END IF
*
*     Undo scaling
*
      IF( IASCL.EQ.1 ) THEN
         CALL ZLASCL( 'G', 0, 0, ANRM, SMLNUM, N, NRHS, B, LDB, INFO )
         CALL DLASCL( 'G', 0, 0, SMLNUM, ANRM, MINMN, 1, S, MINMN,
     $                INFO )
      ELSE IF( IASCL.EQ.2 ) THEN
         CALL ZLASCL( 'G', 0, 0, ANRM, BIGNUM, N, NRHS, B, LDB, INFO )
         CALL DLASCL( 'G', 0, 0, BIGNUM, ANRM, MINMN, 1, S, MINMN,
     $                INFO )
      END IF
      IF( IBSCL.EQ.1 ) THEN
         CALL ZLASCL( 'G', 0, 0, SMLNUM, BNRM, N, NRHS, B, LDB, INFO )
      ELSE IF( IBSCL.EQ.2 ) THEN
         CALL ZLASCL( 'G', 0, 0, BIGNUM, BNRM, N, NRHS, B, LDB, INFO )
      END IF
   70 CONTINUE
      WORK( 1 ) = MAXWRK
      RETURN
*
*     End of ZGELSS
*
      END


SUBROUTINE beam_km(km,ei,ell)
!
! This subroutine forms the stiffness matrix of a
! beam element (bending only).
!
 IMPLICIT NONE
 INTEGER,PARAMETER::iwp=SELECTED_REAL_KIND(15)
 REAL(iwp),INTENT(IN)::ei,ell
 REAL(iwp),INTENT(OUT)::km(:,:)
 REAL(iwp)::two=2.0_iwp,d4=4.0_iwp,d6=6.0_iwp,d12=12.0_iwp
 km(1,1)=d12*ei/(ell*ell*ell)
 km(3,3)=km(1,1)
 km(1,2)=d6*ei/(ell*ell)
 km(2,1)=km(1,2)
 km(1,4)=km(1,2)
 km(4,1)=km(1,4)
 km(1,3)=-km(1,1)
 km(3,1)=km(1,3)
 km(3,4)=-km(1,2)
 km(4,3)=km(3,4)
 km(2,3)=km(3,4)
 km(3,2)=km(2,3)
 km(2,2)=d4*ei/ell
 km(4,4)=km(2,2)
 km(2,4)=two*ei/ell
 km(4,2)=km(2,4)
RETURN
END SUBROUTINE beam_km


SUBROUTINE classico(uint,vint,wint)

	USE velpre
	USE parametros

	IMPLICIT NONE

	!===================================================================================================================
	real(8), dimension(nx1,ny,nz) :: uint
	real(8), dimension(nx,ny1,nz) :: vint
	real(8), dimension(nx,ny,nz1) :: wint

	real(8), dimension(nx1,ny,nz) :: dudx, dudy, dudz, bma, dma, dudxa, dudya, dudza
	real(8), dimension(nx,ny1,nz) :: dvdx, dvdy, dvdz, amb, dmb, dvdxa, dvdya, dvdza
	real(8), dimension(nx,ny,nz1) :: dwdx, dwdy, dwdz, amd, bmd, dwdxa, dwdya, dwdza
	real(8), dimension(nx,ny,nz)  :: aux

	!contadores
	integer :: i, j, k

	!auxiliares
	real(8) :: aux1, aux2

	!===================================================================================================================
	!RESOLUÇÃO DO PROBLEMA
	!===================================================================================================================

	if (der == 1) then
	! upwind

	do k = 1, nz
	do j = 1, ny
	do i = 1, nx1

	bma(i,j,k) = (v(i,j,k) + v(i-1,j,k) + v(i,j+1,k) + v(i-1,j+1,k)) * 0.25
	dma(i,j,k) = (w(i,j,k) + w(i-1,j,k) + w(i,j,k+1) + w(i-1,j,k+1)) * 0.25

	aux1 = max(u(i,j,k),0.)
	aux2 = min(u(i,j,k),0.)
	dudx(i,j,k) = aux1*(u(i,j,k)-u(i-1,j,k))/dx + aux2*(u(i+1,j,k)-u(i,j,k))/dx

	aux1 = max(bma(i,j,k),0.)
	aux2 = min(bma(i,j,k),0.)
	dudy(i,j,k) = aux1*(u(i,j,k)-u(i,j-1,k))/dy + aux2*(u(i,j+1,k)-u(i,j,k))/dy

	aux1 = max(dma(i,j,k),0.)
	aux2 = min(dma(i,j,k),0.)
	dudz(i,j,k) = aux1*(u(i,j,k)-u(i,j,k-1))/dz + aux2*(u(i,j,k+1)-u(i,j,k))/dz

	uint(i,j,k) = dudx(i,j,k) + dudy(i,j,k) + dudz(i,j,k)

	enddo
	enddo
	enddo

	do k = 1, nz
	do j = 1, ny1
	do i = 1, nx

	amb(i,j,k) = (u(i,j,k) + u(i+1,j,k) + u(i,j-1,k) + u(i+1,j-1,k)) * 0.25
	dmb(i,j,k) = (w(i,j,k) + w(i,j-1,k) + w(i,j,k+1) + w(i,j-1,k+1)) * 0.25

	aux1 = max(amb(i,j,k),0.)
	aux2 = min(amb(i,j,k),0.)
	dvdx(i,j,k) = aux1*(v(i,j,k)-v(i-1,j,k))/dx + aux2*(v(i+1,j,k)-v(i,j,k))/dx

	aux1 = max(v(i,j,k),0.)
	aux2 = min(v(i,j,k),0.)
	dvdy(i,j,k) = aux1*(v(i,j,k)-v(i,j-1,k))/dy + aux2*(v(i,j+1,k)-v(i,j,k))/dy

	aux1 = max(dmb(i,j,k),0.)
	aux2 = min(dmb(i,j,k),0.)
	dvdz(i,j,k) = aux1*(v(i,j,k)-v(i,j,k-1))/dz + aux2*(v(i,j,k+1)-v(i,j,k))/dz

	vint(i,j,k) = dvdx(i,j,k) + dvdy(i,j,k) + dvdz(i,j,k)

	enddo
	enddo
	enddo

	do k = 1, nz1
	do j = 1, ny
	do i = 1, nx

	amd(i,j,k) = (u(i,j,k) + u(i+1,j,k) + u(i,j,k-1) + u(i+1,j,k-1)) * 0.25
	bmd(i,j,k) = (v(i,j,k) + v(i,j+1,k) + v(i,j,k-1) + v(i,j+1,k-1)) * 0.25

	aux1 = max(amd(i,j,k),0.)
	aux2 = min(amd(i,j,k),0.)
	dwdx(i,j,k) = aux1*(w(i,j,k)-w(i-1,j,k))/dx + aux2*(w(i+1,j,k)-w(i,j,k))/dx

	aux1 = max(bmd(i,j,k),0.)
	aux2 = min(bmd(i,j,k),0.)
	dwdy(i,j,k) = aux1*(w(i,j,k)-w(i,j-1,k))/dy + aux2*(w(i,j+1,k)-w(i,j,k))/dy

	aux1 = max(w(i,j,k),0.)
	aux2 = min(w(i,j,k),0.)
	dwdz(i,j,k) = aux1*(w(i,j,k)-w(i,j,k-1))/dz + aux2*(w(i,j,k+1)-w(i,j,k))/dz

	wint(i,j,k) = dwdx(i,j,k) + dwdy(i,j,k) + dwdz(i,j,k)

	enddo
	enddo
	enddo

	elseif (der == 2) then

	CALL derivax(u,nx1,ny,nz,dudx)
	CALL derivay(u,nx1,ny,nz,dudy)
	CALL derivaz(u,nx1,ny,nz,dudz)

	CALL derivax(v,nx,ny1,nz,dvdx)
	CALL derivay(v,nx,ny1,nz,dvdy)
	CALL derivaz(v,nx,ny1,nz,dvdz)

	CALL derivax(w,nx,ny,nz1,dwdx)
	CALL derivay(w,nx,ny,nz1,dwdy)
	CALL derivaz(w,nx,ny,nz1,dwdz)

	call interpy_fc(v(1:nx,1:ny1,1:nz),nx,ny1,nz,aux) !(nx,ny,nz)
	call interpx_cf(aux,nx,ny,nz,bma) !(nx1,ny,nz)

	call interpz_fc(w(1:nx,1:ny,1:nz1),nx,ny,nz1,aux) !(nx,ny,nz)
	call interpx_cf(aux,nx,ny,nz,dma) !(nx1,ny,nz)

	call interpx_fc(u(1:nx1,1:ny,1:nz),nx1,ny,nz,aux) !(nx,ny,nz)
	call interpy_cf(aux,nx,ny,nz,amb) !(nx,ny1,nz)

	call interpz_fc(w(1:nx,1:ny,1:nz1),nx,ny,nz1,aux) !(nx,ny,nz)
	call interpy_cf(aux,nx,ny,nz,dmb) !(nx,ny1,nz)

	call interpx_fc(u(1:nx1,1:ny,1:nz),nx1,ny,nz,aux) !(nx,ny,nz)
	call interpz_cf(aux,nx,ny,nz,amd) !(nx,ny,nz1)

	call interpy_fc(v(1:nx,1:ny1,1:nz),nx,ny1,nz,aux) !(nx,ny,nz)
	call interpz_cf(aux,nx,ny,nz,bmd) !(nx,ny,nz1)

	do k = 1, nz
	do j = 1, ny
	do i = 1, nx1
	uint(i,j,k) = u(i,j,k)*dudx(i,j,k) + bma(i,j,k)*dudy(i,j,k) + dma(i,j,k)*dudz(i,j,k)
	enddo
	enddo
	enddo

	do k = 1, nz
	do j = 1, ny1
	do i = 1, nx
	vint(i,j,k) = amb(i,j,k)*dvdx(i,j,k) + v(i,j,k)*dvdy(i,j,k) + dmb(i,j,k)*dvdz(i,j,k)
	enddo
	enddo
	enddo

	do k = 1, nz1
	do j = 1, ny
	do i = 1, nx
	wint(i,j,k) = amd(i,j,k)*dwdx(i,j,k) + bmd(i,j,k)*dwdy(i,j,k) + w(i,j,k)*dwdz(i,j,k)
	enddo
	enddo
	enddo

	elseif (der == 3) then
	! upwind 2nd order

	call interpy_fc(v(1:nx,1:ny1,1:nz),nx,ny1,nz,aux) !(nx,ny,nz)
	call interpx_cf(aux,nx,ny,nz,bma) !(nx1,ny,nz)

	call interpz_fc(w(1:nx,1:ny,1:nz1),nx,ny,nz1,aux) !(nx,ny,nz)
	call interpx_cf(aux,nx,ny,nz,dma) !(nx1,ny,nz)

	call interpx_fc(u(1:nx1,1:ny,1:nz),nx1,ny,nz,aux) !(nx,ny,nz)
	call interpy_cf(aux,nx,ny,nz,amb) !(nx,ny1,nz)

	call interpz_fc(w(1:nx,1:ny,1:nz1),nx,ny,nz1,aux) !(nx,ny,nz)
	call interpy_cf(aux,nx,ny,nz,dmb) !(nx,ny1,nz)

	call interpx_fc(u(1:nx1,1:ny,1:nz),nx1,ny,nz,aux) !(nx,ny,nz)
	call interpz_cf(aux,nx,ny,nz,amd) !(nx,ny,nz1)

	call interpy_fc(v(1:nx,1:ny1,1:nz),nx,ny1,nz,aux) !(nx,ny,nz)
	call interpz_cf(aux,nx,ny,nz,bmd) !(nx,ny,nz1)

	CALL derivaxu2n(u,nx1,ny,nz,dudxa)
	CALL derivaxu2p(u,nx1,ny,nz,dudx)
	CALL derivayu2n(u,nx1,ny,nz,dudya)
	CALL derivayu2p(u,nx1,ny,nz,dudy)
	CALL derivazu2n(u,nx1,ny,nz,dudza)
	CALL derivazu2p(u,nx1,ny,nz,dudz)

	CALL derivaxu2n(v,nx,ny1,nz,dvdxa)
	CALL derivaxu2p(v,nx,ny1,nz,dvdx)
	CALL derivayu2n(v,nx,ny1,nz,dvdya)
	CALL derivayu2p(v,nx,ny1,nz,dvdy)
	CALL derivazu2n(v,nx,ny1,nz,dvdza)
	CALL derivazu2p(v,nx,ny1,nz,dvdz)

	CALL derivaxu2n(w,nx,ny,nz1,dwdxa)
	CALL derivaxu2p(w,nx,ny,nz1,dwdx)
	CALL derivayu2n(w,nx,ny,nz1,dwdya)
	CALL derivayu2p(w,nx,ny,nz1,dwdy)
	CALL derivazu2n(w,nx,ny,nz1,dwdza)
	CALL derivazu2p(w,nx,ny,nz1,dwdz)

	do k = 1, nz
	do j = 1, ny
	do i = 1, nx1

	aux1 = max(u(i,j,k),0.)
	aux2 = min(u(i,j,k),0.)
	dudx(i,j,k) = aux1*dudxa(i,j,k) + aux2*dudx(i,j,k)

	aux1 = max(bma(i,j,k),0.)
	aux2 = min(bma(i,j,k),0.)
	dudy(i,j,k) = aux1*dudya(i,j,k) + aux2*dudy(i,j,k)

	aux1 = max(dma(i,j,k),0.)
	aux2 = min(dma(i,j,k),0.)
	dudz(i,j,k) = aux1*dudza(i,j,k) + aux2*dudz(i,j,k)

	uint(i,j,k) = dudx(i,j,k) + dudy(i,j,k) + dudz(i,j,k)

	enddo
	enddo
	enddo

	do k = 1, nz
	do j = 1, ny1
	do i = 1, nx

	aux1 = max(amb(i,j,k),0.)
	aux2 = min(amb(i,j,k),0.)
	dvdx(i,j,k) = aux1*dvdxa(i,j,k) + aux2*dvdx(i,j,k)

	aux1 = max(v(i,j,k),0.)
	aux2 = min(v(i,j,k),0.)
	dvdy(i,j,k) = aux1*dvdya(i,j,k) + aux2*dvdy(i,j,k)

	aux1 = max(dmb(i,j,k),0.)
	aux2 = min(dmb(i,j,k),0.)
	dvdz(i,j,k) = aux1*dvdza(i,j,k) + aux2*dvdz(i,j,k)

	vint(i,j,k) = dvdx(i,j,k) + dvdy(i,j,k) + dvdz(i,j,k)

	enddo
	enddo
	enddo

	do k = 1, nz1
	do j = 1, ny
	do i = 1, nx

	aux1 = max(amd(i,j,k),0.)
	aux2 = min(amd(i,j,k),0.)
	dwdx(i,j,k) = aux1*dwdxa(i,j,k) + aux2*dwdx(i,j,k)

	aux1 = max(bmd(i,j,k),0.)
	aux2 = min(bmd(i,j,k),0.)
	dwdy(i,j,k) = aux1*dwdya(i,j,k) + aux2*dwdy(i,j,k)

	aux1 = max(w(i,j,k),0.)
	aux2 = min(w(i,j,k),0.)
	dwdz(i,j,k) = aux1*dwdza(i,j,k) + aux2*dwdz(i,j,k)

	wint(i,j,k) = dwdx(i,j,k) + dwdy(i,j,k) + dwdz(i,j,k)

	enddo
	enddo
	enddo

	endif

!==================================================================================================================
END SUBROUTINE classico

SUBROUTINE rotacional(uint,vint,wint)

	USE velpre
	USE parametros

	IMPLICIT NONE

	!===================================================================================================================
	real(8), dimension(nx1,ny,nz) :: uint
	real(8), dimension(nx,ny1,nz) :: vint
	real(8), dimension(nx,ny,nz1) :: wint

	real(8), dimension(0:nx1+1,0:ny1+1,0:nz1+1) :: ap, an, bma, dma
	real(8), dimension(0:nx1+1,0:ny1+1,0:nz1+1) :: bp, bn, amb, dmb
	real(8), dimension(0:nx1+1,0:ny1+1,0:nz1+1) :: dp, dn, amd, bmd

	!
	real(8), dimension(nx1,ny1,nz1) :: dudx, dvdx, dwdx
	real(8), dimension(nx1,ny1,nz1) :: dudy, dvdy, dwdy
	real(8), dimension(nx1,ny1,nz1) :: dudz, dvdz, dwdz

	!
	real(8) :: aa, bb, dd

	!contadores
	integer :: i, j, k, ai, bi, di

	!plotagem
	real(8) :: acont, bcont, dcont
	integer :: loca(3), locb(3), locd(3)

	!auxiliares
	real(8) :: aux1, aux2

	!===================================================================================================================
	!RESOLUÇÃO DO PROBLEMA
	!===================================================================================================================

	if (der == 1) then
	! upwind

	do k = 0, nz+1
	do j = 0, ny+1
	do i = 1, nx+1

	bma(i,j,k) = (v(i,j,k) + v(i-1,j,k) + v(i,j+1,k) + v(i-1,j+1,k)) * 0.25
	dma(i,j,k) = (w(i,j,k) + w(i-1,j,k) + w(i,j,k+1) + w(i-1,j,k+1)) * 0.25

	ap(i,j,k) = max(u(i,j,k),0.)
	an(i,j,k) = min(u(i,j,k),0.)

	bp(i,j,k) = max(bma(i,j,k),0.)
	bn(i,j,k) = min(bma(i,j,k),0.)

	dp(i,j,k) = max(dma(i,j,k),0.)
	dn(i,j,k) = min(dma(i,j,k),0.)

	enddo
	enddo
	enddo

	ap(0,0:ny+1,0:nz+1)     = max(u(0,0:ny+1,0:nz+1),0.)
	an(nx1+1,0:ny+1,0:nz+1) = min(u(nx1+1,0:ny+1,0:nz+1),0.)

	do k = 1, nz
	do j = 1, ny
	do i = 1, nx1

	dudx(i,j,k) = (ap(i,j,k)*u(i,j,k)-ap(i-1,j,k)*u(i-1,j,k))/dx + (an(i+1,j,k)*u(i+1,j,k)-an(i,j,k)*u(i,j,k))/dx
	dudy(i,j,k) = (bp(i,j,k)*u(i,j,k)-bp(i,j-1,k)*u(i,j-1,k))/dy + (bn(i,j+1,k)*u(i,j+1,k)-bn(i,j,k)*u(i,j,k))/dy
	dudz(i,j,k) = (dp(i,j,k)*u(i,j,k)-dp(i,j,k-1)*u(i,j,k-1))/dz + (dn(i,j,k+1)*u(i,j,k+1)-dn(i,j,k)*u(i,j,k))/dz

	uint(i,j,k) = dudx(i,j,k) + dudy(i,j,k) + dudz(i,j,k)

	enddo
	enddo
	enddo

	do k = 0, nz+1
	do j = 1, ny+1
	do i = 0, nx+1

	amb(i,j,k) = (u(i,j,k) + u(i+1,j,k) + u(i,j-1,k) + u(i+1,j-1,k)) * 0.25
	dmb(i,j,k) = (w(i,j,k) + w(i,j-1,k) + w(i,j,k+1) + w(i,j-1,k+1)) * 0.25

	ap(i,j,k) = max(amb(i,j,k),0.)
	an(i,j,k) = min(amb(i,j,k),0.)

	bp(i,j,k) = max(v(i,j,k),0.)
	bn(i,j,k) = min(v(i,j,k),0.)

	dp(i,j,k) = max(dmb(i,j,k),0.)
	dn(i,j,k) = min(dmb(i,j,k),0.)

	enddo
	enddo
	enddo

	bp(0:nx+1,0,0:nz+1)     = max(v(0:nx+1,0,0:nz+1),0.)
	bn(0:nx+1,ny1+1,0:nz+1) = min(v(0:nx+1,ny1+1,0:nz+1),0.)

	do k = 1, nz
	do j = 1, ny1
	do i = 1, nx

	dvdx(i,j,k) = (ap(i,j,k)*v(i,j,k)-ap(i-1,j,k)*v(i-1,j,k))/dx + (an(i+1,j,k)*v(i+1,j,k)-an(i,j,k)*v(i,j,k))/dx
	dvdy(i,j,k) = (bp(i,j,k)*v(i,j,k)-bp(i,j-1,k)*v(i,j-1,k))/dy + (bn(i,j+1,k)*v(i,j+1,k)-bn(i,j,k)*v(i,j,k))/dy
	dvdz(i,j,k) = (dp(i,j,k)*v(i,j,k)-dp(i,j,k-1)*v(i,j,k-1))/dz + (dn(i,j,k+1)*v(i,j,k+1)-dn(i,j,k)*v(i,j,k))/dz

	vint(i,j,k) = dvdx(i,j,k) + dvdy(i,j,k) + dvdz(i,j,k)

	enddo
	enddo
	enddo

	do k = 1, nz+1
	do j = 0, ny+1
	do i = 0, nx+1

	amd(i,j,k) = (u(i,j,k) + u(i+1,j,k) + u(i,j,k-1) + u(i+1,j,k-1)) * 0.25
	bmd(i,j,k) = (v(i,j,k) + v(i,j+1,k) + v(i,j,k-1) + v(i,j+1,k-1)) * 0.25

	ap(i,j,k) = max(amd(i,j,k),0.)
	an(i,j,k) = min(amd(i,j,k),0.)

	bp(i,j,k) = max(bmd(i,j,k),0.)
	bn(i,j,k) = min(bmd(i,j,k),0.)

	dp(i,j,k) = max(w(i,j,k),0.)
	dn(i,j,k) = min(w(i,j,k),0.)

	enddo
	enddo
	enddo

	dp(0:nx+1,0:ny+1,0)     = max(w(0:nx+1,0:ny+1,0),0.)
	dp(0:nx+1,0:ny+1,nz1+1) = min(w(0:nx+1,0:ny+1,nz1+1),0.)
	do k = 1, nz1
	do j = 1, ny
	do i = 1, nx

	dwdx(i,j,k) = (ap(i,j,k)*w(i,j,k)-ap(i-1,j,k)*w(i-1,j,k))/dx + (an(i+1,j,k)*w(i+1,j,k)-an(i,j,k)*w(i,j,k))/dx
	dwdy(i,j,k) = (bp(i,j,k)*w(i,j,k)-bp(i,j-1,k)*w(i,j-1,k))/dy + (bn(i,j+1,k)*w(i,j+1,k)-bn(i,j,k)*w(i,j,k))/dy
	dwdz(i,j,k) = (dp(i,j,k)*w(i,j,k)-dp(i,j,k-1)*w(i,j,k-1))/dz + (dn(i,j,k+1)*w(i,j,k+1)-dn(i,j,k)*w(i,j,k))/dz

	wint(i,j,k) = dwdx(i,j,k) + dwdy(i,j,k) + dwdz(i,j,k)

	enddo
	enddo
	enddo

	else

	write(*,*) "não possui este esquema para o rotacional"
	STOP

	endif

!==================================================================================================================
END SUBROUTINE rotacional

SUBROUTINE antissim(uint,vint,wint)

	USE velpre
	USE parametros

	IMPLICIT NONE

	!===================================================================================================================
	real(8), dimension(nx1,ny,nz) :: uint
	real(8), dimension(nx,ny1,nz) :: vint
	real(8), dimension(nx,ny,nz1) :: wint

	real(8), dimension(0:nx1+1,0:ny1+1,0:nz1+1) :: ap, an
	real(8), dimension(0:nx1+1,0:ny1+1,0:nz1+1) :: bp, bn
	real(8), dimension(0:nx1+1,0:ny1+1,0:nz1+1) :: dp, dn

	!
	real(8), dimension(nx1,ny1,nz1) :: dudx, dvdx, dwdx
	real(8), dimension(nx1,ny1,nz1) :: dudy, dvdy, dwdy
	real(8), dimension(nx1,ny1,nz1) :: dudz, dvdz, dwdz
	real(8), dimension(nx1,ny1,nz1) :: bma, dma,amb, dmb, amd, bmd

	!
	real(8) :: aa, bb, dd

	!contadores
	integer :: i, j, k, ai, bi, di

	!plotagem
	real(8) :: acont, bcont, dcont
	integer :: loca(3), locb(3), locd(3)

	!auxiliares
	real(8) :: aux1, aux2

	!===================================================================================================================
	!RESOLUÇÃO DO PROBLEMA
	!===================================================================================================================

	if (der == 1) then
	! upwind

	do k = 1, nz+1
	do j = 1, ny+1
	do i = 1, nx+1
	bma(i,j,k) = (v(i,j,k) + v(i-1,j,k) + v(i,j+1,k) + v(i-1,j+1,k)) * 0.25
	dma(i,j,k) = (w(i,j,k) + w(i-1,j,k) + w(i,j,k+1) + w(i-1,j,k+1)) * 0.25

	ap(i,j,k) = max(u(i,j,k),0.)
	an(i,j,k) = min(u(i,j,k),0.)

	bp(i,j,k) = max(bma(i,j,k),0.)
	bn(i,j,k) = min(bma(i,j,k),0.)

	dp(i,j,k) = max(dma(i,j,k),0.)
	dn(i,j,k) = min(dma(i,j,k),0.)
	enddo
	enddo
	enddo

	ap(0,:,:) = ap(1,:,:)
	bp(:,0,:) = bp(:,1,:)
	dp(:,:,0) = dp(:,:,1)
        an(nx1+1,:,:)=an(nx1,:,:)

	do k = 1, nz
	do j = 1, ny
	do i = 1, nx1

	dudx(i,j,k) = ((ap(i,j,k)*u(i,j,k)-ap(i-1,j,k)*u(i-1,j,k))/dx + (an(i+1,j,k)*u(i+1,j,k)-an(i,j,k)*u(i,j,k))/dx &
	+ ap(i,j,k)*(u(i,j,k)-u(i-1,j,k))/dx + an(i,j,k)*(u(i+1,j,k)-u(i,j,k))/dx) * 0.5

	dudy(i,j,k) = ((bp(i,j,k)*u(i,j,k)-bp(i,j-1,k)*u(i,j-1,k))/dy + (bn(i,j+1,k)*u(i,j+1,k)-bn(i,j,k)*u(i,j,k))/dy &
	+ bp(i,j,k)*(u(i,j,k)-u(i,j-1,k))/dy + bn(i,j,k)*(u(i,j+1,k)-u(i,j,k))/dy) * 0.5

	dudz(i,j,k) = ((dp(i,j,k)*u(i,j,k)-dp(i,j,k-1)*u(i,j,k-1))/dz+(dn(i,j,k+1)*u(i,j,k+1)-dn(i,j,k)*u(i,j,k))/dz &
	+ dp(i,j,k)*(u(i,j,k)-u(i,j,k-1))/dz + dn(i,j,k)*(u(i,j,k+1)-u(i,j,k))/dz) * 0.5

	uint(i,j,k) = dudx(i,j,k) + dudy(i,j,k) + dudz(i,j,k)

	enddo
	enddo
	enddo

	do k = 1, nz+1
	do j = 1, ny+1
	do i = 1, nx+1

	amb(i,j,k) = (u(i,j,k) + u(i+1,j,k) + u(i,j-1,k) + u(i+1,j-1,k)) * 0.25
	dmb(i,j,k) = (w(i,j,k) + w(i,j-1,k) + w(i,j,k+1) + w(i,j-1,k+1)) * 0.25

	ap(i,j,k) = max(amb(i,j,k),0.)
	an(i,j,k) = min(amb(i,j,k),0.)

	bp(i,j,k) = max(v(i,j,k),0.)
	bn(i,j,k) = min(v(i,j,k),0.)

	dp(i,j,k) = max(dmb(i,j,k),0.)
	dn(i,j,k) = min(dmb(i,j,k),0.)

	enddo
	enddo
	enddo

	ap(0,:,:) = ap(1,:,:)
	bp(:,0,:) = bp(:,1,:)
	dp(:,:,0) = dp(:,:,1)
        bn(:,ny1+1,:)=bn(:,ny1,:)

	do k = 1, nz
	do j = 1, ny1
	do i = 1, nx

	dvdx(i,j,k) = ((ap(i,j,k)*v(i,j,k)-ap(i-1,j,k)*v(i-1,j,k))/dx + (an(i+1,j,k)*v(i+1,j,k)-an(i,j,k)*v(i,j,k))/dx &
	+ ap(i,j,k)*(v(i,j,k)-v(i-1,j,k))/dx + an(i,j,k)*(v(i+1,j,k)-v(i,j,k))/dx) * 0.5

	dvdy(i,j,k) = ((bp(i,j,k)*v(i,j,k)-bp(i,j-1,k)*v(i,j-1,k))/dy + (bn(i,j+1,k)*v(i,j+1,k)-bn(i,j,k)*v(i,j,k))/dy &
	+ bp(i,j,k)*(v(i,j,k)-v(i,j-1,k))/dy + bn(i,j,k)*(v(i,j+1,k)-v(i,j,k))/dy) * 0.5

	dvdz(i,j,k) = ((dp(i,j,k)*v(i,j,k)-dp(i,j,k-1)*v(i,j,k-1))/dz+(dn(i,j,k+1)*v(i,j,k+1)-dn(i,j,k)*v(i,j,k))/dz &
	+ dp(i,j,k)*(v(i,j,k)-v(i,j,k-1))/dz + dn(i,j,k)*(v(i,j,k+1)-v(i,j,k))/dz) * 0.5

	vint(i,j,k) = dvdx(i,j,k) + dvdy(i,j,k) + dvdz(i,j,k)

	enddo
	enddo
	enddo

	do k = 1, nz+1
	do j = 1, ny+1
	do i = 1, nx+1

	amd(i,j,k) = (u(i,j,k) + u(i+1,j,k) + u(i,j,k-1) + u(i+1,j,k-1)) * 0.25
	bmd(i,j,k) = (v(i,j,k) + v(i,j+1,k) + v(i,j,k-1) + v(i,j+1,k-1)) * 0.25

	ap(i,j,k) = max(amd(i,j,k),0.)
	an(i,j,k) = min(amd(i,j,k),0.)

	bp(i,j,k) = max(bmd(i,j,k),0.)
	bn(i,j,k) = min(bmd(i,j,k),0.)

	dp(i,j,k) = max(w(i,j,k),0.)
	dn(i,j,k) = min(w(i,j,k),0.)

	enddo
	enddo
	enddo

	ap(0,:,:) = ap(1,:,:)
	bp(:,0,:) = bp(:,1,:)
	dp(:,:,0) = dp(:,:,1)
        dn(:,:,nz1+1)=dn(:,:,nz1)

	do k = 1, nz1
	do j = 1, ny
	do i = 1, nx

	dwdx(i,j,k) = ((ap(i,j,k)*w(i,j,k)-ap(i-1,j,k)*w(i-1,j,k))/dx + (an(i+1,j,k)*w(i+1,j,k)-an(i,j,k)*w(i,j,k))/dx &
	+ ap(i,j,k)*(w(i,j,k)-w(i-1,j,k))/dx + an(i,j,k)*(w(i+1,j,k)-w(i,j,k))/dx) * 0.5

	dwdy(i,j,k) = ((bp(i,j,k)*w(i,j,k)-bp(i,j-1,k)*w(i,j-1,k))/dy + (bn(i,j+1,k)*w(i,j+1,k)-bn(i,j,k)*w(i,j,k))/dy &
	+ bp(i,j,k)*(w(i,j,k)-w(i,j-1,k))/dy + bn(i,j,k)*(w(i,j+1,k)-w(i,j,k))/dy) * 0.5

	dwdz(i,j,k) = ((dp(i,j,k)*w(i,j,k)-dp(i,j,k-1)*w(i,j,k-1))/dz+(dn(i,j,k+1)*w(i,j,k+1)-dn(i,j,k)*w(i,j,k))/dz &
	+ dp(i,j,k)*(w(i,j,k)-w(i,j,k-1))/dz + dn(i,j,k)*(w(i,j,k+1)-w(i,j,k))/dz) * 0.5

	wint(i,j,k) = dwdx(i,j,k) + dwdy(i,j,k) + dwdz(i,j,k)

	enddo
	enddo
	enddo

	else

	write(*,*) "não possui esquema para o antissimétrico"
	STOP

	endif

!==================================================================================================================
END SUBROUTINE antissim


      double complex function pvfndd(n,x,iep)
C----Implementation of DD Eq. 4.11
      implicit none
      include 'TRonshellcutoff.f'
      integer j,n,infty
      double complex xm1,x,cln,cone
      double precision iep
      parameter(cone=(1d0,0d0),infty=16) ! number of terms in sum

      xm1=x-cone
      if (abs(x) .lt. 10d0) then
        if (abs(x-cone) .lt. onshellcutoff) then
          pvfndd=0d0
        else
          pvfndd=(cone-x**(n+1))*(cln(xm1,iep)-cln(x,iep))
        endif
        do j=0,n
          pvfndd=pvfndd-x**(n-j)/dfloat(j+1)
        enddo
      else
        pvfndd=cln(cone-cone/x,iep)
        do j=n+1,n+infty
          pvfndd=pvfndd+x**(n-j)/dfloat(j+1)
        enddo
      endif

      return
      end


program tmod

!  This is the test progrm for the F-90 based MP translation modules.

!  David H. Bailey   2004-07-12

use mpmodule
implicit type (mp_real) (a-h, o-z)
type (mp_integer) ia, ib, ic
type (mp_complex) c, d, e
parameter (n = 25)
dimension a(n), b(n)

call mpinit (1000)

!   Character-to-MP assignment, generic MPREAL function, pi and e.

x = '1.234567890 1234567890 1234567890 D-100'
ee = exp (mpreal (1.d0))
call mpwrite (6, x, mppic, ee)
s = 0.d0

!   Loop with subscripted MP variables.

do i = 1, n
  a(i) = 2 * i + 1
  b(i) = 2.d0 * a(i) * (a(i) + 1.d0)
  s = s + b(i) ** 2
enddo
call mpwrite (6, s)

!   Expression with mixed MPI and MPR entities.

ia = s
ib = 262144
s = (s + 327.25d0) * mod (ia, 4 * ib)
call mpwrite (6, s)

!   A complex square root reference.

e = sqrt (mpcmpl (2.d0 * s, s))
call mpwrite (6, e)

!   External and intrinsic MP function references in expressions.

s = dot (n, a, b)
t = 2.d0 * sqrt (s) ** 2
call mpwrite (6, s, t)

s = s / 1048576.d0
t = s + 2.d0 * log (s)
x = 3 + nint (t) * 5
call mpwrite (6, s, t, x)

!   A deeply nested expression with function references.

x = (s + (2 * (s - 5) + 3 * (t - 5))) * exp (cos (log (s)))
call mpwrite (6, x)

!   A special MP subroutine call (computes both cos and sin of S).

call mpcssnf (s, x, y)
t = 1.d0 - (x ** 2 + y ** 2)

!   IF-THEN-ELSE construct involving MP variables.

if (abs (t) .lt. mpeps) then
  call mpwrite (6, t)
else
  call mpwrite (6, mpeps)
endif

stop
end

function dot (n, a, b)

!   MP function subprogram.

use mpmodule
type (mp_real) a(n), b(n), dot, s

s = 0.d0

do i = 1, n
  s = s + a(i) * b(i)
enddo

dot = s
return
end


!!!! Chuong trinh tinh standard deviation

   PROGRAM standard_deviation
   IMPLICIT NONE

   INTEGER (KIND=8),PARAMETER::N=32768
   INTEGER (KIND=8):: i,j,ia,ib
   REAL    (KIND=8):: Sa_av,dev_a,Sb_av,dev_b,pi

   REAL (KIND=8),DIMENSION(N) :: Ks,S,Sa,Sb

!!!=======================================================================================
!!!=======================================================================================

   pi=acos(-1.)

   OPEN(unit=11,file='phi_equi.dat')
   OPEN(unit=12,file='phi_av.txt')

   S(:)=0. ; Sa(:)=0. ; Sb(:)=0.

!!! Doc gia tri
   ia=0 ; ib=0
   DO i=1,N
      READ(11,*)j,Ks(j),S(j)

      S(i)=S(i)-int(S(i)/2./pi)*2.*pi

      IF (S(i)<0.) THEN
         S(i)=S(i)+2.*pi
      END IF

      IF (Ks(j)>0.) THEN
         ia=ia+1
         Sa(ia)=S(j)
      ELSE
         ib=ib+1
         Sb(ib)=S(j)
      END IF
   END DO

!!! Tinh gia tri trung binh
   Sa_av=0.
   DO i=1,ia
      Sa_av=Sa_av+Sa(i)
   END DO
   Sa_av=Sa_av/ia

   Sb_av=0.
   DO i=1,ib
      Sb_av=Sb_av+Sb(i)
   END DO
   Sb_av=Sb_av/ib

!!! Tinh standard deviation
   dev_a=0.
   DO i=1,ia
      dev_a=dev_a+(Sa(i)-Sa_av)**2.
   END DO
   dev_a=dev_a/ia

   dev_b=0.
   DO i=1,ib
      dev_b=dev_b+(Sb(i)-Sb_av)**2.
   END DO
   dev_b=dev_b/ib

   WRITE(12,*)N,ia,ib
   WRITE(12,*)Sa_av,dev_a,Sb_av,dev_b

   CLOSE(11)
   CLOSE(12)

   END PROGRAM standard_deviation


program demo_math_all_close
    use forlab_math,  only: all_close
    use stdlib_error, only: check
    real    :: x(2) = [1, 2], random(4, 4)
    complex :: z(4, 4)

    call check(all_close(x, [2.0, 2.0], rel_tol=1.0e-6, abs_tol=1.0e-3), &
               msg="all_close(x, [2.0, 2.0]) failed.", warn=.true.)
               !! all_close(x, [2.0, 2.0]) failed.
    call random_number(random(4, 4))
    z = 1.0
    print *, all_close(z+1.0e-11*random, z)     !! T

end program demo_math_all_close


program test
use easy_scalapack
implicit none
integer, parameter :: p = 2, q = 2, m = 9, n = 9, mb = 2, nb = 2
type(process_grid) :: pc
type(dense_matrix) :: dm, b
type(subarray)     :: sa, sb
integer            :: i, j, info
real               :: rnd

pc = process_grid(p,q)
dm = dense_matrix( m, n, mb, nb, pc )
b = dense_matrix( m, 1, mb, nb, pc )

!call dm%print_mapping()

do i = 1, dm%mg
    do j = 1, dm%ng
        call random_number(rnd)
        call dm%set( i, j, rnd )
    end do
    call b%set( i, 1, 1.0 )
end do

!call dm%print_matrix(6)

sa = subarray( dm )

! LU factorization
call psgetrf( sa%m, sa%n, sa%la, sa%ia, sa%ja, sa%desc, sa%ipiv, info )
if( info /= 0 ) call pc%stop_all("LU factorization unsuccessful")

sb = subarray(b)
call psgetrs( 'N', sa%m, 1, sa%la, sa%ia, sa%ja, sa%desc, sa%ipiv, sb%la, sb%ia, sb%ja, sb%desc, info )

!call psgetri( sa%n, sa%la, sa%ia, sa%ja, sa%desc, sa%ipiv, sa%work, sa%lwork, sa%iwork, sa%liwork, info ) still problematic!

call b%print_matrix(6)

call pc%destroy()
call pc%stop_all()

end program


C     * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
C     *                                                               *
C     *                  copyright (c) 1999 by UCAR                   *
C     *                                                               *
C     *       UNIVERSITY CORPORATION for ATMOSPHERIC RESEARCH         *
C     *                                                               *
C     *                      all rights reserved                      *
C     *                                                               *
C     *                      FISHPACK version 4.1                     *
C     *                                                               *
C     *     A PACKAGE OF FORTRAN SUBPROGRAMS FOR THE SOLUTION OF      *
C     *                                                               *
C     *      SEPARABLE ELLIPTIC PARTIAL DIFFERENTIAL EQUATIONS        *
C     *                                                               *
C     *                             BY                                *
C     *                                                               *
C     *        JOHN ADAMS, PAUL SWARZTRAUBER AND ROLAND SWEET         *
C     *                                                               *
C     *                             OF                                *
C     *                                                               *
C     *         THE NATIONAL CENTER FOR ATMOSPHERIC RESEARCH          *
C     *                                                               *
C     *                BOULDER, COLORADO  (80307)  U.S.A.             *
C     *                                                               *
C     *                   WHICH IS SPONSORED BY                       *
C     *                                                               *
C     *              THE NATIONAL SCIENCE FOUNDATION                  *
C     *                                                               *
C     * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
C
C
C
C     EXAMPLE SHOWING THE USE OF SEPX4 TO SOLVE THE ELLIPTIC PDE
C     (X+1)**2*UXX+2*(X+1)*UX+UYY-X*U=G(X,Y) ON THE REGION
C     0.LE.X.LE.1, 0.LE.Y.LE.1  WITH SPECIFIED BOUNDARY CONDITIONS
C     AT Y=0,1 AND MIXED BOUNDARY CONDITIONS OF THE FORM
C     UX(0,Y)+U(0,Y), UX(1,Y)+U(1,Y) AT X=0,1.
C     THE APPROXIMATION IS GENERATED ON A UNIFORM 33 BY 33 GRID.
C     THE EXACT SOLUTION U(X,Y)=(X*Y)**3+1 IS USED TO SET THE
C     RIGHT HAND SIDE, BOUNDARY CONDITIONS, AND COMPUTE  SECOND AND
C     FOURTH ORDER DISCRETIZATION ERROR
C     THE EXACT WORK SPACE LENGTH REQUIRED IS 1005 WORDS.  THIS
C     WAS DETERMINED BY A PREVIOUS CALL TO SEPX4 AND PRINT OUT OF
C     W(1).
C
      DIMENSION       USOL(33,33),GRHS(33,33),BDA(33)    ,BDB(33)    ,
     1                W(1024)
      EXTERNAL COFX4
C
C     DEFINE ARITHMETIC FUNCTIONS GIVING EXACT SOLUTION
C
      UE(S,T)=(S*T)**3+1.0
      UXE(S,T)=3.0*S**2*T**3
      UXXE(S,T)=6.0*S*T**3
      UYE(S,T)=3.0*S**3*T**2
      UYYE(S,T)=6.0*S**3*T
C
C     SET LIMITS ON REGION
C
      A = 0.0
      B = 1.0
      C = 0.0
      D = 1.0
C
C     SET GRID SIZE
C
      M = 32
      N = 32
      DLX = (B-A)/FLOAT(M)
      DLY = (D-C)/FLOAT(N)
      NX = M+1
      NY = N+1
          DO 102 I=1,NX
          X = A+FLOAT(I-1)*DLX
C
C     SET SPECIFIED BOUNDARY CONDITIONS AT Y=C,D
C
          USOL(I,1) = UE(X,C)
          USOL(I,NY) = UE(X,D)
          CALL COFX4 (X,AF,BF,CF)
              DO 101 J=1,NY
              Y = C+FLOAT(J-1)*DLY
C
C     SET RIGHT HAND SIDE
C
              GRHS(I,J) = AF*UXXE(X,Y)+BF*UXE(X,Y)+CF*UE(X,Y)+UYYE(X,Y)
  101         CONTINUE
  102     CONTINUE
C
C     SET MIXED BOUNDARY CONDITIONS AT X=A,B
C
      ALPHA = 1.0
      BETA = 1.0
          DO 103 J=1,NY
          Y = C+FLOAT(J-1)*DLY
          BDA(J) = UXE(A,Y)+ALPHA*UE(A,Y)
          BDB(J) = UXE(B,Y)+BETA*UE(B,Y)
  103     CONTINUE
C
C     SET BOUNDARY SWITHCES
C
      MBDCND = 3
      NBDCND = 1
C
C     SET FIRST DIMENSION OF USOL,GRHS AND WORK SPACE LENGTH
C
      IDMN = 33
      W(1) = 1024.
C
C     OBTAIN SECOND ORDER APPROXIMATION
C
      IORDER = 2
      CALL SEPX4 (IORDER,A,B,M,MBDCND,BDA,ALPHA,BDB,BETA,C,D,N,NBDCND,
     1            DUM,DUM,COFX4,GRHS,USOL,IDMN,W,PERTRB,IERROR)
C
C     COMPUTE SECOND ORDER DISCRETIZATION ERROR (RELATIVE)
C     ALSO RESET SPECIFIED BOUNDARIES AND RIGHT HAND SIDE.
C
      ERR = 0.0
          DO 105 I=1,NX
          X = A+FLOAT(I-1)*DLX
          USOL(I,1) = UE(X,C)
          USOL(I,NY) = UE(X,D)
          CALL COFX4 (X,AF,BF,CF)
              DO 104 J=1,NY
              Y = C+FLOAT(J-1)*DLY
              ERR = AMAX1(ERR,ABS((USOL(I,J)-UE(X,Y))/UE(X,Y)))
              GRHS(I,J) = AF*UXXE(X,Y)+BF*UXE(X,Y)+CF*UE(X,Y)+UYYE(X,Y)
  104         CONTINUE
  105     CONTINUE
      ERR2=ERR
C
C     OBTAIN FOURTH ORDER APPROXIMATION
C
      IORDER = 4
      CALL SEPX4 (IORDER,A,B,M,MBDCND,BDA,ALPHA,BDB,BETA,C,D,N,NBDCND,
     1            DUM,DUM,COFX4,GRHS,USOL,IDMN,W,PERTRB,IERROR)
C
C     COMPUTE FOURTH ORDER DISCRETIZATION ERROR (RELATIVE)
C
      ERR = 0.0
          DO 107 J=1,NY
          Y = C+FLOAT(J-1)*DLY
              DO 106 I=1,NX
              X = A+FLOAT(I-1)*DLX
              ERR = AMAX1(ERR,ABS((USOL(I,J)-UE(X,Y))/UE(X,Y)))
  106         CONTINUE
  107     CONTINUE
      ERR4=ERR
      IW = INT(W(1)+0.5)
      PRINT 1001,IERROR,ERR2,ERR4,IW
      STOP IERROR

 1001 FORMAT(1H1,20X,25HSUBROUTINE SEPX4  EXAMPLE ///
     120X,46HTHE OUTPUT FROM THE NCAR CONTROL DATA 7600 WAS //
     220X,10HIERROR = 0 /
     320X,48HSECOND ORDER DISCRETIZATION ERROR =  1.5985E-04  /
     420X,48HFOURTH ORDER DISCRETIZATION ERROR =  1.85749E-06  /
     520X,33HREQUIRED LENGTH OF W ARRAY = 1024 //
     620X, 32HTHE OUTPUT FROM YOUR COMPUTER IS //
     720X, 8HIERROR = I2 /
     820X,36HSECOND ORDER DISCRETIZATION ERROR = E12.5 /
     920X,36HFOURTH ORDER DISCRETIZATION ERROR = E12.5 /
     920X,29HREQUIRED LENGTH OF W ARRAY = I5)
C
      END
      SUBROUTINE COFX4(X,AF,BF,CF)
C
C     SET COEFFICIENTS IN THE X-DIRECTION.
C
      AF = (X+1.)**2
      BF = 2.0*(X+1.)
      CF = -X
      RETURN
      END


FUNCTION gammq(a,x)
REAL a,gammq,x
!CU    USES gcf,gser
REAL gammcf,gamser,gln
if(x.lt.0..or.a.le.0.)pause 'bad arguments in gammq'
if(x.lt.a+1.)then
  call gser(gamser,a,x,gln)
  gammq=1.-gamser
else
  call gcf(gammcf,a,x,gln)
  gammq=gammcf
endif
return
END


      SUBROUTINE slXYXY (X1,Y1,COEFFS,X2,Y2)
*+
*     - - - - - -
*      X Y X Y
*     - - - - - -
*
*  Transform one [X,Y] into another using a linear model of the type
*  produced by the slFTXY routine.
*
*  Given:
*     X1       d        x-coordinate
*     Y1       d        y-coordinate
*     COEFFS  d(6)      transformation coefficients (see note)
*
*  Returned:
*     X2       d        x-coordinate
*     Y2       d        y-coordinate
*
*  The model relates two sets of [X,Y] coordinates as follows.
*  Naming the elements of COEFFS:
*
*     COEFFS(1) = A
*     COEFFS(2) = B
*     COEFFS(3) = C
*     COEFFS(4) = D
*     COEFFS(5) = E
*     COEFFS(6) = F
*
*  the present routine performs the transformation:
*
*     X2 = A + B*X1 + C*Y1
*     Y2 = D + E*X1 + F*Y1
*
*  See also slFTXY, slPXY, slINVF, slDCMF
*
*  P.T.Wallace   Starlink   5 December 1994
*
*  Copyright (C) 1995 Rutherford Appleton Laboratory
*
*  License:
*    This program is free software; you can redistribute it and/or modify
*    it under the terms of the GNU General Public License as published by
*    the Free Software Foundation; either version 2 of the License, or
*    (at your option) any later version.
*
*    This program is distributed in the hope that it will be useful,
*    but WITHOUT ANY WARRANTY; without even the implied warranty of
*    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
*    GNU General Public License for more details.
*
*    You should have received a copy of the GNU General Public License
*    along with this program (see SLA_CONDITIONS); if not, write to the
*    Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor,
*    Boston, MA  02110-1301  USA
*
*  Copyright (C) 1995 Association of Universities for Research in Astronomy Inc.
*-

      IMPLICIT NONE

      DOUBLE PRECISION X1,Y1,COEFFS(6),X2,Y2

      X2=COEFFS(1)+COEFFS(2)*X1+COEFFS(3)*Y1
      Y2=COEFFS(4)+COEFFS(5)*X1+COEFFS(6)*Y1

      END


	FUNCTION PR_HGFS  ( hgft )
C************************************************************************
C* PR_HGFS								*
C*									*
C* This function computes HGML, height in miles, from HGFT.  The 	*
C* following equation is used:						*
C*									*
C*               HGML = HGFT / 5280.    				*
C*									*
C* REAL PR_HGFS  ( HGFT )						*
C*									*
C* Input parameters:							*
C*	HGFT		REAL	 	Height in feet          	*
C*									*
C* Output parameters:							*
C*	PR_HGFS		REAL		Height in statute miles 	*
C**									*
C* Log:									*
C* S. Schotz    	10/89	Original source				*
C************************************************************************
        INCLUDE    'GEMPRM.PRM'
C
	REAL       ft2ml
	PARAMETER  (ft2ml = 1.0 / 5280.0)
C
        INCLUDE    'ERMISS.FNC'
C------------------------------------------------------------------------
C*	Check for missing values.
C
	IF  ( ERMISS ( hgft ) )  THEN
	    PR_HGFS = RMISSD
	  ELSE
	    PR_HGFS = hgft * ft2ml
	END IF
C*
	RETURN
	END


!> Orchid -- 2D/3D Euler/MagnetoHydroDynamics solver.
!> Copyright (C) Butakov Oleg 2019.

Module orchid_solver_grid_gauss_const
Implicit None
Contains
!########################################################################################################
!########################################################################################################
!########################################################################################################
Function mhd_grid_gauss_point(Npoints, k) Result(x)
    !> Points of the Gauss quadrature.
    !> {{{
    Integer, Intent(In) :: Npoints
    Integer, Intent(In) :: k
    Real(8) :: x
    !> }}}
    Real(8), Dimension(1:1), Parameter :: x1 = [ +0.000000000D0 ]
    Real(8), Dimension(1:2), Parameter :: x2 = [ -0.577350269D0, +0.577350269D0 ]
    Real(8), Dimension(1:3), Parameter :: x3 = [ -0.774596669D0, +0.000000000D0, +0.774596669D0 ]
    Real(8), Dimension(1:4), Parameter :: x4 = [ -0.861136312D0, -0.339981044D0, +0.339981044D0, &
                                                 +0.861136312D0 ]
    Real(8), Dimension(1:5), Parameter :: x5 = [ -0.906179846D0, -0.538469310D0, +0.000000000D0, &
                                                 +0.538469310D0, +0.906179846D0 ]
    Select Case ( Npoints )
        Case ( 1 )
            x = x1(k)
        Case ( 2 )
            x = x2(k)
        Case ( 3 )
            x = x3(k)
        Case ( 4 )
            x = x4(k)
        Case ( 5 )
            x = x5(k)
        Case Default
            Write (0,*) 'Too many points for Gauss quadratures.'
            Error Stop -1
    End Select
End Function mhd_grid_gauss_point
!########################################################################################################
!########################################################################################################
!########################################################################################################
Function mhd_grid_gauss_weight(Npoints, k) Result(w)
    !> Weights of the Gauss quadrature.
    !> {{{
    Integer, Intent(In) :: Npoints
    Integer, Intent(In) :: k
    Real(8) :: w
    !> }}}
    Real(8), Dimension(1:1), Parameter :: w1 = [ +1.000000000D0 ]
    Real(8), Dimension(1:2), Parameter :: w2 = [ +0.500000000D0, +0.50000000D0 ]
    Real(8), Dimension(1:3), Parameter :: w3 = [ +0.277777778D0, +0.444444444D0, +0.277777778D0 ]
    Real(8), Dimension(1:4), Parameter :: w4 = [ +0.173927423D0, +0.326072577D0, +0.326072577D0, &
                                                 +0.173927423D0 ]
    Real(8), Dimension(1:5), Parameter :: w5 = [ +0.118463443D0, +0.239314335D0, +0.284444444D0, &
                                                 +0.239314335D0, +0.118463443D0 ]
    Select Case ( Npoints )
        Case ( 1 )
            w = w1(k)
        Case ( 2 )
            w = w2(k)
        Case ( 3 )
            w = w3(k)
        Case ( 4 )
            w = w4(k)
        Case ( 5 )
            w = w5(k)
        Case Default
            Write (0,*) 'Too many points for Gauss quadratures.'
            Error Stop -1
    End Select
End Function mhd_grid_gauss_weight
!########################################################################################################
!########################################################################################################
!########################################################################################################
End Module orchid_solver_grid_gauss_const

Module orchid_solver_grid_gauss
Use orchid_solver_params
Use orchid_solver_grid
Use orchid_solver_grid_gauss_const
Implicit None
Type :: MhdGridGaussNode
    Real(8) :: x, y, z, w
    Contains
End Type MhdGridGaussNode
Type :: MhdGridGaussCell
    Integer :: nnode, nnode_end
    Contains
End Type MhdGridGaussCell
Type :: MhdGridGaussFace
    Integer :: nnode, nnode_end
    Contains
End Type MhdGridGaussFace
Type, Extends(MhdGrid) :: MhdGridGauss
    Integer :: ncell_nodes_min, ncell_nodes_max
    Integer :: nface_nodes_min, nface_nodes_max
    Type(MhdGridGaussCell), Dimension(:), Allocatable :: gcells
    Type(MhdGridGaussFace), Dimension(:), Allocatable :: gfaces
    Type(MhdGridGaussNode), Dimension(:), Allocatable :: cell_nodes, face_nodes
    Contains
    Procedure, Public, Non_Overridable :: init_gauss => mhd_grid_gauss_init
    Procedure, Public, Non_Overridable :: init_gauss1D => mhd_grid_gauss_init1D
    Procedure, Public, Non_Overridable :: init_gauss_dummy => mhd_grid_gauss_init_dummy
End Type MhdGridGauss
Private :: mhd_grid_gauss_init1D, &
           mhd_grid_gauss_init_dummy
Contains
!########################################################################################################
!########################################################################################################
!########################################################################################################
Subroutine mhd_grid_gauss_init(This, Npoints)
    !> Initialize a Gauss grid.
    !> {{{
    Class(MhdGridGauss), Intent(InOut) :: This
    Integer, Intent(In), Optional :: Npoints
    !> }}}
    If ( Present(Npoints) ) Then
        Select Case ( This%ndims )
            Case ( 1 )
                Call This%init_gauss1D(Npoints)
            !Case ( 2 )
            !    Call This%init_gauss2D(Npoints)
            !Case ( 3 )
            !    Call This%init_gauss3D(Npoints)
            Case Default
                Write (0,*) 'Invalid amount of the grid dimensions. ', &
                            'Was grid initialized?'
                Error Stop -1
        End Select
    Else
        Call This%init_gauss_dummy()
    End If
End Subroutine mhd_grid_gauss_init
!########################################################################################################
!########################################################################################################
!########################################################################################################
Subroutine mhd_grid_gauss_init1D(This, Npoints)
    !> Initialize a 1D Gauss grid.
    !> {{{
    Class(MhdGridGauss), Intent(InOut) :: This
    Integer, Intent(In) :: Npoints
    !> }}}
    Integer :: n, k, kk
    Real(8) :: h, x
    !>-------------------------------------------------------------------------------
    !> Allocate the Gauss Grid.
    This%ncell_nodes_min = This%ncells_min
    This%ncell_nodes_max = This%ncells_max*Npoints
    This%nface_nodes_min = This%nfaces_min
    This%nface_nodes_max = This%nfaces_max
    Allocate(This%gcells(This%ncells_min:This%ncells_max))
    Allocate(This%gfaces(This%nfaces_min:This%nfaces_max))
    Allocate(This%cell_nodes(This%ncell_nodes_min:This%ncell_nodes_max))
    Allocate(This%face_nodes(This%nface_nodes_min:This%nface_nodes_max))
    !>-------------------------------------------------------------------------------

    !>-------------------------------------------------------------------------------
    !> Initialize Cells and CellNodes.
    !$OMP Parallel Do Private(k, kk, h, x)
    Do n = This%ncells_min, This%ncells_max
        h = This%cells(n)%Vcell
        x = This%nodes(This%cell2node(This%cells(n)%nnode))%x
        This%gcells(n)%nnode = Npoints*( n - 1 ) + 1
        This%gcells(n)%nnode_end = Npoints*n
        Do k = This%gcells(n)%nnode, This%gcells(n)%nnode_end
            kk = k - This%gcells(n)%nnode + 1
            This%cell_nodes(k)%w = mhd_grid_gauss_weight(Npoints, kk)
            This%cell_nodes(k)%x = mhd_grid_gauss_point(Npoints, kk)
            This%cell_nodes(k)%x = x + 0.5D0*h*( This%cell_nodes(k)%x + 1.0D0 )
            This%cell_nodes(k)%y = 0.0D0
            This%cell_nodes(k)%z = 0.0D0
        End Do
    End Do
    !$OMP End Parallel Do
    !> Initialize Faces and FaceNodes.
    !$OMP Parallel Do
    Do n = This%nfaces_min, This%nfaces_max
        This%gfaces(n)%nnode = n
        This%gfaces(n)%nnode_end = n
        This%face_nodes(n)%x = This%faces(n)%x
        This%face_nodes(n)%y = This%faces(n)%y
        This%face_nodes(n)%z = This%faces(n)%z
        This%face_nodes(n)%w = 1.0D0
    End Do
    !$OMP End Parallel Do
    !>-------------------------------------------------------------------------------
End Subroutine mhd_grid_gauss_init1D
!########################################################################################################
!########################################################################################################
!########################################################################################################
Subroutine mhd_grid_gauss_init_dummy(This)
    !> Initialize a dummy Gauss grid (for first-order integration).
    !> {{{
    Class(MhdGridGauss), Intent(InOut) :: This
    !> }}}
    Integer :: n
    !>-------------------------------------------------------------------------------
    !> Allocate the Gauss Grid.
    This%ncell_nodes_min = This%ncells_min
    This%ncell_nodes_max = This%ncells_max
    This%nface_nodes_min = This%nfaces_min
    This%nface_nodes_max = This%nfaces_max
    Allocate(This%gcells(This%ncells_min:This%ncells_max))
    Allocate(This%gfaces(This%nfaces_min:This%nfaces_max))
    Allocate(This%cell_nodes(This%ncell_nodes_min:This%ncell_nodes_max))
    Allocate(This%face_nodes(This%nface_nodes_min:This%nface_nodes_max))
    !>-------------------------------------------------------------------------------

    !>-------------------------------------------------------------------------------
    !> Initialize Cells and CellNodes.
    !$OMP Parallel Do
    Do n = This%ncells_min, This%ncells_max
        This%gcells(n)%nnode = n
        This%gcells(n)%nnode_end = n
        This%cell_nodes(n)%x = This%cells(n)%x
        This%cell_nodes(n)%y = This%cells(n)%y
        This%cell_nodes(n)%z = This%cells(n)%z
        This%cell_nodes(n)%w = 1.0D0
    End Do
    !$OMP End Parallel Do
    !> Initialize Faces and FaceNodes.
    !$OMP Parallel Do
    Do n = This%nfaces_min, This%nfaces_max
        This%gfaces(n)%nnode = n
        This%gfaces(n)%nnode_end = n
        This%face_nodes(n)%x = This%faces(n)%x
        This%face_nodes(n)%y = This%faces(n)%y
        This%face_nodes(n)%z = This%faces(n)%z
        This%face_nodes(n)%w = 1.0D0
    End Do
    !$OMP End Parallel Do
    !>-------------------------------------------------------------------------------
End Subroutine mhd_grid_gauss_init_dummy
!########################################################################################################
!########################################################################################################
!########################################################################################################
End Module orchid_solver_grid_gauss


module gaussinte
  implicit none
  double precision, private, parameter :: pai = 4.0d0 * atan(1.0d0)
  private
  public gaussinte_w, gaussinte_fx, gaussinte_fv

contains

!   Description: Gauss quadrature for the following integral

!      | b
!      |  f(x) dx
!      | a

!   gaussinte_w: make points and weights for n discrete points
!     n (in): the number of points. This is the size of both x and w.
!     x (out): x-axis points in the interval [-1, 1].
!     w (out): weights for the points.
!     eps (in): tolerance error.

!   gaussinte_fx: make x points for the interval [a,b]
!     n (in): the number of points.
!     x (in): the x points for interval [-1, 1].
!     a, b (in): lower and upper points for the integral
!     fx (out): x point for the interval [a, b]
!     return value: (b-a)/2

!   gaussinte_fv: compute the integral
!     n (in): the number of points.
!     w (in): weights for the x points in [-1,1]
!     c (in): (b-a)/2 ?
!     fv (in): function values at x points derived by gauss_inte_fx
!     return value: the interal value

  subroutine gaussinte_w(n, x, w, eps)
    integer, intent(in) :: n
    double precision, intent(out) :: x(1:n), w(1:n)
    double precision, intent(in) :: eps

    integer :: m, i, l
    double precision :: npai, tmp, dt
    double precision :: p0, p1, p2
    double precision :: q0, q1, q2

    select case (n)
      case (1)
        x(1) = 0.0d0
        w(1) = 2.0d0
        return
      case (2)
        x(1) = -sqrt(1.0d0/3.0d0)
        w(1) = 1.0d0
        x(2) = -x(1)
        w(2) = w(1)
        return
      case (3)
        x(1) = -sqrt(0.6d0)
        w(1) = 5.0d0/9.0d0
        x(2) = 0.0d0
        w(2) = 8.0d0/9.0d0
        x(3) = -x(1)
        w(3) = w(1)
        return
    end select

    m = n / 2
    npai = pai / (dble(n) + 0.5d0)
    do i = 1, m
      tmp = cos((i - 0.25d0) * npai)
      dt = tmp
      do while (abs(dt) > abs(tmp) * eps)
        p1 = tmp
        p2 = (3.0d0 * tmp**2 - 1.0d0) * 0.5d0
        q1 = 1.0d0
        q2 = 3.0d0 * tmp
        do l = 3, n
          p0 = p1
          p1 = p2
          p2 = ((l + l - 1) * tmp * p1 - (l-1) * p0) / l
          q0 = q1
          q1 = q2
          q2 = ((l + l - 1) * (tmp * q1 + p1) - (l-1) * q0) / l
        end do
        dt = p2 / q2
        tmp = tmp - dt
      end do
      x(i) = tmp
      w(i) = 2.0d0 / (n * p1 * q2)
    end do

    if (mod(n, 2) == 1) then
      tmp = dble(n)
      do i = 1, m
        tmp = tmp * (0.5d0 - i) / dble(i)
      end do
      x(m+1) = 0.0d0
      w(m+1) = 2.0d0 / tmp**2
    end if

    do i = 1, m
      x(n+1-i) = x(i)
      x(i) = -x(i) ! reverse order
      w(n+1-i) = w(i)
    end do
  end subroutine gaussinte_w

  function gaussinte_fx(n, x, a, b, fx) result(t1)
    integer, intent(in) :: n
    double precision, intent(in) :: x(1:n), a, b
    double precision, intent(out) :: fx(1:n)

    integer :: i
    double precision :: t1, t2

    t1 = (b - a)/2.0d0
    t2 = (b + a)/2.0d0
    do i = 1, n
      fx(i) = t1 * x(i) + t2
    end do
  end function gaussinte_fx

  function gaussinte_fv(n, w, c, fv) result(s)
    integer, intent(in) :: n
    double precision, intent(in) :: w(1:n), c, fv(1:n)

    integer :: i
    double precision :: s

    s = 0.0d0
    do i = 1, n
      s = s + w(i) * fv(i)
    end do
    s = s * c
  end function gaussinte_fv

end module gaussinte


! ==========================================================
! satellites_nonlinear.f95
!
! This program calculates the control and state of a
! fully actuated satellite using optimal control with
! quadratic cost
!	I(u) = 1/2 * integ( x*Qx + u*Ru ) dt
!
! And constraints on control
! 	ui_min <= u_i <= ui_max,  i=1,2,3.
!
! It uses the fully non-linear model
!	Jw' = -w x Jw + u
!	q' = 1/2 * Omega(w) * q
!
! INPUT:
! - "input_file": This file is created by the user.
! It contains the simulation parameters as follows:
!  T_0: Initial time
!  T_1: Final time
!  j1 j2 j3: Moments of inertia
!  q1 q2 q3 q4 q5 q6 q7: Elemets of diagonal matrix q
!  r1 r2 r3: Elements of diagonal matrix R
!  omega_0(1) omega_0(2), omega_0(3): Initial ang. vel.
!  omega_1(1) omega_1(2), omega_1(3): Final ang. vel.
!  eul_0(1) eul_0(2) eul_0(3): Initial Euler angles
!  eul_1(1) eul_1(2) eul_1(3): Final Euler angles
!  m: Number of intervals in the discretization
!  u1min u2min u3min u1max u2max u3max: Control constraints
!  mask(1) ... mask(7): Natural/Essential bound. constraints
!  prec: Precission of the algorithm
!
! OUTPUT:
! - "results_control": Control
! - "results_angveloc": Angular velocities
! - "results_eulerang": Euler angles
! - "results_quaternion": Quaternion
! - "results_quatnorm": Norm of quaternion
! - "results_time": Time vector
! - "results_converg": Convergence
! - "results_misc": Miscelaneous results (Cost, CPU time...)
!
! The convention for quaternion and
! Euler angles is as follows:
! q = (epsilon,eta)
! eul = (phi,theta,rho) = (yaw,pitch,roll)
! ==========================================================

! ======== INITIALIZATION ========
! Variables declares here are passed to all rubroutines
module initialization
implicit none
integer:: N, m, nn
real:: T_0, T_1
real:: j1, j2, j3
real:: q1, q2, q3, q4, q5, q6, q7
real:: r1, r2, r3
real:: u1min, u1max, u2min, u2max, u3min, u3max
end module initialization

! ======== MAIN ========
program main
use initialization
implicit none

integer, allocatable:: mask(:)
real, allocatable:: state_0(:), state_1(:), state(:, :)
integer:: i
real:: cpu_clock_ini, cpu_clock_fin
real:: prec, cost, eulang(3), pi
real:: omega_0(3), omega_1(3)
real:: eul_0(3), eul_1(3), quat_0(4), quat_1(4)
N=7; ! Dimension of state
nn=14;  ! Number of constraints
pi = 4*atan(1.)
allocate (state_0(N), state_1(N), mask(N))

! Open input file
open (11,file='input_file',status='old')
! Read input parameters
read(11,*) T_0
read(11,*) T_1
read(11,*) j1, j2, j3
read(11,*) q1, q2, q3, q4, q5, q6, q7
read(11,*) r1, r2, r3
read(11,*) omega_0(1), omega_0(2), omega_0(3)
read(11,*) omega_1(1), omega_1(2), omega_1(3)
read(11,*) eul_0(1), eul_0(2), eul_0(3)
read(11,*) eul_1(1), eul_1(2), eul_1(3)
read(11,*) m
read(11,*) u1min, u2min, u3min, u1max, u2max, u3max
read(11,*) mask(1), mask(2), mask(3), mask(4),&
	 mask(5), mask(6), mask(7)
read(11,*) prec
close(11)

allocate (state(m+1,N))

call eul2quat(eul_0,quat_0)
call eul2quat(eul_1,quat_1)
state_0 = (/omega_0,quat_0/)
state_1 = (/omega_1,quat_1/)

do i=1, m+1
	state(i, :)=((m+1.-i)/m)*state_0+((i-1.)/m)*state_1
end do

call CPU_TIME(cpu_clock_ini)
call optimalcontrol(prec, mask, state, cost)
call CPU_TIME(cpu_clock_fin)

open(11,file='results_misc', &
status='replace',action='write')
write(11,*) 'NONLINEAR MODEL;'
write(11,*) 'Initial time T_0;', t_0
write(11,*) 'Final time T_1;', t_1
write(11,*) 'Moments of inertia;', j1, j2, j3
write(11,*) 'Q (diagonal matix);', q1, q2, q3, q4, q5, q6
write(11,*) 'R (diagonal matix);', r1, r2, r3
write(11,*) 'Initial angular velocity w_0;', omega_0
write(11,*) 'Initial angular velocity w_1;', omega_1
write(11,*) 'Initial orientation (yaw,pitch,roll);', eul_0
write(11,*) 'Final orientation (yaw,pitch,roll);', eul_1
write(11,*) '# Intervals discretization;', m
write(11,*) 'Precision;', prec
write(11,*) 'Mask (1=bound.cond./0=natural);', mask
write(11,*) 'Cost I(u);', cost
write(11,*) 'CPU time (s);', cpu_clock_fin-cpu_clock_ini
close(11)

open(11,file='results_control', &
status='replace',action='write')
do i = 1,m
	write(11,*) &
	(state(i+1,1) - state(i,1))*m/(T_1-T_0), &
	(state(i+1,2) - state(i,2))*m/(T_1-T_0), &
	(state(i+1,3) - state(i,3))*m/(T_1-T_0)
end do
close(11)

open(11,file='results_quaternion', &
status='replace',action='write')
do i = 1,m+1
	write(11,*) state(i,4:7)
end do
close(11)
open(11,file='results_eulerang', &
status='replace',action='write')

do i = 1,m+1
	call quat2eul(state(i,4:7),eulang)
	write (11,*) eulang
end do
close(11)

open(11,file='results_angveloc', &
status='replace',action='write')
do i = 1,m+1
	write (11,*) state(i,1:3)
end do
close(11)

open(11,file='results_quatnorm', &
status='replace',action='write')
do i = 1, m+1
        write (11,*) &
	state(i,4)**2+state(i,5)**2+ &
	state(i,6)**2+state(i,7)**2
end do
close (11)

open(11,file='results_time', &
status='replace',action='write')
do i = 1, m+1
        write (11,*) T_0+(T_1-T_0)*(i-1)/m
end do
close (11)

deallocate (state_0, state_1, state, mask)
end program main

! ======== CONSTRAINTS ========
subroutine constraints(s, x, p, R)
use initialization
implicit none
real:: s, x(N), p(N), R(nn)
R (1)  =  j1*p(1) + (j3-j2)*(x(2)*x(3)) - u1max
R (2)  = -j1*p(1) - (j3-j2)*(x(2)*x(3)) + u1min
R (3)  =  j2*p(2) + (j1-j3)*(x(1)*x(3)) - u2max
R (4)  = -j2*p(2) - (j1-j3)*(x(1)*x(3)) + u2min
R (5)  =  j3*p(3) + (j2-j1)*(x(1)*x(2)) - u3max
R (6)  = -j3*p(3) - (j2-j1)*(x(1)*x(2)) + u3min
R (7)  =  p(4) - .5*(+ x(1)*x(7) - x(2)*x(6) + x(3)*x(5))
R (8)  = -p(4) + .5*(+ x(1)*x(7) - x(2)*x(6) + x(3)*x(5))
R (9)  =  p(5) - .5*(+ x(1)*x(6) - x(3)*x(4) + x(2)*x(7))
R (10) = -p(5) + .5*(+ x(1)*x(6) - x(3)*x(4) + x(2)*x(7))
R (11) =  p(6) - .5*(+ x(2)*x(4) - x(1)*x(5) + x(3)*x(7))
R (12) = -p(6) + .5*(+ x(2)*x(4) - x(1)*x(5) + x(3)*x(7))
R (13) =  p(7) - .5*(- x(1)*x(4) - x(2)*x(5) - x(3)*x(6))
R (14) = -p(7) + .5*(- x(1)*x(4) - x(2)*x(5) - x(3)*x(6))
end subroutine constraints

! ======== INTEGRAND ========
subroutine integrand(s, x, p, y, F)
use initialization
implicit none
real, intent(in):: s, x(N), p(N), y(m, nn)
real, intent(out):: F
real:: l(nn), R(nn)
integer:: i
call constraints(s, x, p, R)
call indexfunction(s, y, l)

F =     (r1*(j1*p(1) - j2*x(2)*x(3) + j3*x(2)*x(3))**2)/2
F = F + (r2*(j2*p(2) + j1*x(1)*x(3) - j3*x(1)*x(3))**2)/2
F = F + (r3*(j3*p(3) - j1*x(1)*x(2) + j2*x(1)*x(2))**2)/2

F = F + (q1*x(1)**2)/2
F = F + (q2*x(2)**2)/2
F = F + (q3*x(3)**2)/2
F = F + (q4*x(4)**2)/2
F = F + (q5*x(5)**2)/2
F = F + (q6*x(6)**2)/2
F = F + (q7*x(7)**2)/2
F = F - nn
do i = 1,nn
        F = F + exp(l(i)*R(i))
end do
end subroutine integrand

! ======== AUX INTEGRAND 1 ========
subroutine auxintegrand1(s, x, p, y, H)
use initialization
implicit none
real, intent(in)::  s, x(N), p(N), y(m, nn)
real, intent(out):: H(N)
real:: l(nn)!, R(nn)
!call constraints(s, x, p, R)
call indexfunction(s, y, l)
H(1)=q1*x(1)-(l(7)*x(7)*exp(l(7)*(p(4)-&
	(x(1)*x(7))/2+(x(2)*x(6))/2-(x(3)*x(5))/2)))/2
H(1)=H(1)+(l(8)*x(7)*exp(-l(8)*(p(4)-&
	(x(1)*x(7))/2+(x(2)*x(6))/2-(x(3)*x(5))/2)))/2
H(1)=H(1)-(l(9)*x(6)*exp(l(9)*(p(5)-&
	(x(1)*x(6))/2+(x(3)*x(4))/2-(x(2)*x(7))/2)))/2
H(1)=H(1)+(l(10)*x(6)*exp(-l(10)*(p(5)-&
	(x(1)*x(6))/2+(x(3)*x(4))/2-(x(2)*x(7))/2)))/2
H(1)=H(1)+(l(11)*x(5)*exp(l(11)*(p(6)+&
	(x(1)*x(5))/2-(x(2)*x(4))/2-(x(3)*x(7))/2)))/2
H(1)=H(1)-(l(12)*x(5)*exp(-l(12)*(p(6)+&
	(x(1)*x(5))/2-(x(2)*x(4))/2-(x(3)*x(7))/2)))/2
H(1)=H(1)+(l(13)*x(4)*exp(l(13)*(p(7)+&
	(x(1)*x(4))/2+(x(2)*x(5))/2+(x(3)*x(6))/2)))/2
H(1)=H(1)-(l(14)*x(4)*exp(-l(14)*(p(7)+&
	(x(1)*x(4))/2+(x(2)*x(5))/2+(x(3)*x(6))/2)))/2
H(1)=H(1)-r3*(j1*x(2)-j2*x(2))*&
	(j3*p(3)-j1*x(1)*x(2)+j2*x(1)*x(2))
H(1)=H(1)+r2*(j1*x(3)-j3*x(3))*&
	(j2*p(2)+j1*x(1)*x(3)-j3*x(1)*x(3))
H(1)=H(1)+l(3)*x(3)*&
	exp(l(3)*(j2*p(2)+x(1)*x(3)*(j1-j3)-1))*(j1-j3)
H(1)=H(1)-l(4)*x(3)*&
	exp(-l(4)*(j2*p(2)+x(1)*x(3)*(j1-j3)+1))*(j1-j3)
H(1)=H(1)-l(5)*x(2)*&
	exp(-l(5)*(x(1)*x(2)*(j1-j2)-j3*p(3)+1))*(j1-j2)
H(1)=H(1)+l(6)*x(2)*&
	exp(-l(6)*(j3*p(3)-x(1)*x(2)*(j1-j2)+1))*(j1-j2)

H(2)=q2*x(2)+(l(7)*x(6)*exp(l(7)*(p(4)-&
	(x(1)*x(7))/2+(x(2)*x(6))/2-(x(3)*x(5))/2)))/2
H(2)=H(2)-(l(8)*x(6)*exp(-l(8)*(p(4)-&
	(x(1)*x(7))/2+(x(2)*x(6))/2-(x(3)*x(5))/2)))/2
H(2)=H(2)-(l(9)*x(7)*exp(l(9)*(p(5)-&
	(x(1)*x(6))/2+(x(3)*x(4))/2-(x(2)*x(7))/2)))/2
H(2)=H(2)-(l(11)*x(4)*exp(l(11)*(p(6)+&
	(x(1)*x(5))/2-(x(2)*x(4))/2-(x(3)*x(7))/2)))/2
H(2)=H(2)+(l(10)*x(7)*exp(-l(10)*(p(5)-&
	(x(1)*x(6))/2+(x(3)*x(4))/2-(x(2)*x(7))/2)))/2
H(2)=H(2)+(l(12)*x(4)*exp(-l(12)*(p(6)+&
	(x(1)*x(5))/2-(x(2)*x(4))/2-(x(3)*x(7))/2)))/2
H(2)=H(2)+(l(13)*x(5)*exp(l(13)*(p(7)+&
	(x(1)*x(4))/2+(x(2)*x(5))/2+(x(3)*x(6))/2)))/2
H(2)=H(2)-(l(14)*x(5)*exp(-l(14)*(p(7)+&
	(x(1)*x(4))/2+(x(2)*x(5))/2+(x(3)*x(6))/2)))/2
H(2)=H(2)-r3*(j1*x(1)-j2*x(1))*&
	(j3*p(3)-j1*x(1)*x(2)+j2*x(1)*x(2))
H(2)=H(2)-r1*(j2*x(3)-j3*x(3))*&
	(j1*p(1)-j2*x(2)*x(3)+j3*x(2)*x(3))
H(2)=H(2)-l(1)*x(3)*&
	exp(-l(1)*(x(2)*x(3)*(j2-j3)-j1*p(1)+1))*(j2-j3)
H(2)=H(2)+l(2)*x(3)*&
	exp(-l(2)*(j1*p(1)-x(2)*x(3)*(j2-j3)+1))*(j2-j3)
H(2)=H(2)-l(5)*x(1)*&
	exp(-l(5)*(x(1)*x(2)*(j1-j2)-j3*p(3)+1))*(j1-j2)
H(2)=H(2)+l(6)*x(1)*&
	exp(-l(6)*(j3*p(3)-x(1)*x(2)*(j1-j2)+1))*(j1-j2)

H(3)=q3*x(3)-(l(7)*x(5)*exp(l(7)*(p(4)-&
	(x(1)*x(7))/2+(x(2)*x(6))/2-(x(3)*x(5))/2)))/2
H(3)=H(3)+(l(8)*x(5)*exp(-l(8)*(p(4)-&
	(x(1)*x(7))/2+(x(2)*x(6))/2-(x(3)*x(5))/2)))/2
H(3)=H(3)+(l(9)*x(4)*exp(l(9)*(p(5)-&
	(x(1)*x(6))/2+(x(3)*x(4))/2-(x(2)*x(7))/2)))/2
H(3)=H(3)-(l(10)*x(4)*exp(-l(10)*(p(5)-&
	(x(1)*x(6))/2+(x(3)*x(4))/2-(x(2)*x(7))/2)))/2
H(3)=H(3)-(l(11)*x(7)*exp(l(11)*(p(6)+&
	(x(1)*x(5))/2-(x(2)*x(4))/2-(x(3)*x(7))/2)))/2
H(3)=H(3)+(l(12)*x(7)*exp(-l(12)*(p(6)+&
	(x(1)*x(5))/2-(x(2)*x(4))/2-(x(3)*x(7))/2)))/2
H(3)=H(3)+(l(13)*x(6)*exp(l(13)*(p(7)+&
	(x(1)*x(4))/2+(x(2)*x(5))/2+(x(3)*x(6))/2)))/2
H(3)=H(3)-(l(14)*x(6)*exp(-l(14)*(p(7)+&
	(x(1)*x(4))/2+(x(2)*x(5))/2+(x(3)*x(6))/2)))/2
H(3)=H(3)+r2*(j1*x(1)-j3*x(1))*&
	(j2*p(2)+j1*x(1)*x(3)-j3*x(1)*x(3))
H(3)=H(3)-r1*(j2*x(2)-j3*x(2))*&
	(j1*p(1)-j2*x(2)*x(3)+j3*x(2)*x(3))
H(3)=H(3)-l(1)*x(2)*&
	exp(-l(1)*(x(2)*x(3)*(j2-j3)-j1*p(1)+1))*(j2-j3)
H(3)=H(3)+l(3)*x(1)*&
	exp(l(3)*(j2*p(2)+x(1)*x(3)*(j1-j3)-1))*(j1-j3)
H(3)=H(3)+l(2)*x(2)*&
	exp(-l(2)*(j1*p(1)-x(2)*x(3)*(j2-j3)+1))*(j2-j3)
H(3)=H(3)-l(4)*x(1)*&
	exp(-l(4)*(j2*p(2)+x(1)*x(3)*(j1-j3)+1))*(j1-j3)

H(4)=q4*x(4)+(l(9)*x(3)*exp(l(9)*(p(5)-&
	(x(1)*x(6))/2+(x(3)*x(4))/2-(x(2)*x(7))/2)))/2
H(4)=H(4)-(l(10)*x(3)*exp(-l(10)*(p(5)-&
	(x(1)*x(6))/2+(x(3)*x(4))/2-(x(2)*x(7))/2)))/2
H(4)=H(4)-(l(11)*x(2)*exp(l(11)*(p(6)+&
	(x(1)*x(5))/2-(x(2)*x(4))/2-(x(3)*x(7))/2)))/2
H(4)=H(4)+(l(12)*x(2)*exp(-l(12)*(p(6)+&
	(x(1)*x(5))/2-(x(2)*x(4))/2-(x(3)*x(7))/2)))/2
H(4)=H(4)+(l(13)*x(1)*exp(l(13)*(p(7)+&
	(x(1)*x(4))/2+(x(2)*x(5))/2+(x(3)*x(6))/2)))/2
H(4)=H(4)-(l(14)*x(1)*exp(-l(14)*(p(7)+&
	(x(1)*x(4))/2+(x(2)*x(5))/2+(x(3)*x(6))/2)))/2

H(5)=q5*x(5)-(l(7)*x(3)*exp(l(7)*(p(4)-&
	(x(1)*x(7))/2+(x(2)*x(6))/2-(x(3)*x(5))/2)))/2
H(5)=H(5)+(l(8)*x(3)*exp(-l(8)*(p(4)-&
	(x(1)*x(7))/2+(x(2)*x(6))/2-(x(3)*x(5))/2)))/2
H(5)=H(5)+(l(11)*x(1)*exp(l(11)*(p(6)+&
	(x(1)*x(5))/2-(x(2)*x(4))/2-(x(3)*x(7))/2)))/2
H(5)=H(5)-(l(12)*x(1)*exp(-l(12)*(p(6)+&
	(x(1)*x(5))/2-(x(2)*x(4))/2-(x(3)*x(7))/2)))/2
H(5)=H(5)+(l(13)*x(2)*exp(l(13)*(p(7)+&
	(x(1)*x(4))/2+(x(2)*x(5))/2+(x(3)*x(6))/2)))/2
H(5)=H(5)-(l(14)*x(2)*exp(-l(14)*(p(7)+&
	(x(1)*x(4))/2+(x(2)*x(5))/2+(x(3)*x(6))/2)))/2

H(6)=q6*x(6)+(l(7)*x(2)*exp(l(7)*(p(4)-&
	(x(1)*x(7))/2+(x(2)*x(6))/2-(x(3)*x(5))/2)))/2
H(6)=H(6)-(l(8)*x(2)*exp(-l(8)*(p(4)-&
	(x(1)*x(7))/2+(x(2)*x(6))/2-(x(3)*x(5))/2)))/2
H(6)=H(6)-(l(9)*x(1)*exp(l(9)*(p(5)-&
	(x(1)*x(6))/2+(x(3)*x(4))/2-(x(2)*x(7))/2)))/2
H(6)=H(6)+(l(10)*x(1)*exp(-l(10)*(p(5)-&
	(x(1)*x(6))/2+(x(3)*x(4))/2-(x(2)*x(7))/2)))/2
H(6)=H(6)+(l(13)*x(3)*exp(l(13)*(p(7)+&
	(x(1)*x(4))/2+(x(2)*x(5))/2+(x(3)*x(6))/2)))/2
H(6)=H(6)-(l(14)*x(3)*exp(-l(14)*(p(7)+&
	(x(1)*x(4))/2+(x(2)*x(5))/2+(x(3)*x(6))/2)))/2

H(7)=q7*x(7)-(l(7)*x(1)*exp(l(7)*(p(4)-&
	(x(1)*x(7))/2+(x(2)*x(6))/2-(x(3)*x(5))/2)))/2
H(7)=H(7)+(l(8)*x(1)*exp(-l(8)*(p(4)-&
	(x(1)*x(7))/2+(x(2)*x(6))/2-(x(3)*x(5))/2)))/2
H(7)=H(7)-(l(9)*x(2)*exp(l(9)*(p(5)-&
	(x(1)*x(6))/2+(x(3)*x(4))/2-(x(2)*x(7))/2)))/2
H(7)=H(7)+(l(10)*x(2)*exp(-l(10)*(p(5)-&
	(x(1)*x(6))/2+(x(3)*x(4))/2-(x(2)*x(7))/2)))/2
H(7)=H(7)-(l(11)*x(3)*exp(l(11)*(p(6)+&
	(x(1)*x(5))/2-(x(2)*x(4))/2-(x(3)*x(7))/2)))/2
H(7)=H(7)+(l(12)*x(3)*exp(-l(12)*(p(6)+&
	(x(1)*x(5))/2-(x(2)*x(4))/2-(x(3)*x(7))/2)))/2
end subroutine auxintegrand1

! ======== AUX INTEGRAND 2 ========
subroutine auxintegrand2(s, x, p, y, T, G)
use initialization
implicit none
real, intent(in):: s, x(N), p(N), y(m, nn), T
real, intent(out):: G(N)
integer:: i
real:: l(nn), H(N)!, R(nn)
!call constraints(s, x, p, R)
call indexfunction(s, y, l)
call auxintegrand1(s, x, p, y, H)
G(1)=j1*r1*(j1*p(1)-j2*x(2)*x(3)+j3*x(2)*x(3))
G(1)=G(1)+j1*l(1)*exp(-l(1)*(x(2)*x(3)*(j2-j3)-j1*p(1)+1))
G(1)=G(1)-j1*l(2)*exp(-l(2)*(j1*p(1)-x(2)*x(3)*(j2-j3)+1))
G(1)=G(1)-(T-s)*H(1)

G(2)=j2*r2*(j2*p(2)+j1*x(1)*x(3)-j3*x(1)*x(3))
G(2)=G(2)+j2*l(3)*exp(l(3)*(j2*p(2)+x(1)*x(3)*(j1-j3)-1))
G(2)=G(2)-j2*l(4)*exp(-l(4)*(j2*p(2)+x(1)*x(3)*(j1-j3)+1))
G(2)=G(2)-(T-s)*H(2)

G(3)=j3*r3*(j3*p(3)-j1*x(1)*x(2)+j2*x(1)*x(2))
G(3)=G(3)+j3*l(5)*exp(-l(5)*(x(1)*x(2)*(j1-j2)-j3*p(3)+1))
G(3)=G(3)-j3*l(6)*exp(-l(6)*(j3*p(3)-x(1)*x(2)*(j1-j2)+1))
G(3)=G(3)-(T-s)*H(3)

G(4)=l(7)*exp(l(7)*(p(4)-(x(1)*x(7))/2+&
	(x(2)*x(6))/2-(x(3)*x(5))/2))
G(4)=G(4)-l(8)*exp(-l(8)*(p(4)-(x(1)*x(7))/2+&
	(x(2)*x(6))/2-(x(3)*x(5))/2))
G(4)=G(4)-(T-s)*H(4)

G(5)=l(9)*exp(l(9)*(p(5)-(x(1)*x(6))/2+&
	(x(3)*x(4))/2-(x(2)*x(7))/2))
G(5)=G(5)-l(10)*exp(-l(10)*(p(5)-(x(1)*x(6))/2+&
	(x(3)*x(4))/2-(x(2)*x(7))/2))
G(5)=G(5)-(T-s)*H(5)

G(6)=l(11)*exp(l(11)*(p(6)+(x(1)*x(5))/2-&
	(x(2)*x(4))/2-(x(3)*x(7))/2))
G(6)=G(6)-l(12)*exp(-l(12)*(p(6)+(x(1)*x(5))/2-&
	(x(2)*x(4))/2-(x(3)*x(7))/2))
G(6)=G(6)-(T-s)*H(6)

G(7)=l(13)*exp(l(13)*(p(7)+(x(1)*x(4))/2+&
	(x(2)*x(5))/2+(x(3)*x(6))/2))
G(7)=G(7)-l(14)*exp(-l(14)*(p(7)+(x(1)*x(4))/2+&
	(x(2)*x(5))/2+(x(3)*x(6))/2))
G(7)=G(7)-(T-s)*H(7)
end subroutine auxintegrand2

! ======== EULER ANGLES TO QUATERNION ========
subroutine eul2quat(eul,q)
real, intent(in):: eul(3)
real, intent(out):: q(4)
real:: cy, sy, cp, sp, cr, sr
cy = cos(eul(1)*.5);
sy = sin(eul(1)*.5);
cp = cos(eul(2)*.5);
sp = sin(eul(2)*.5);
cr = cos(eul(3)*.5);
sr = sin(eul(3)*.5);
q(1) = cy * cp * sr - sy * sp * cr
q(2) = sy * cp * sr + cy * sp * cr
q(3) = sy * cp * cr - cy * sp * sr
q(4) = cy * cp * cr + sy * sp * sr
end subroutine eul2quat

! ======== QUATERNION TO EULER ANGLES ========
subroutine quat2eul(q,eul)
real, intent(in):: q(4)
real, intent(out):: eul(3)
eul(1) = atan2(2.*(q(4)*q(3)+q(1)*q(2)), &
	1.-2.*(q(2)**2.+q(3)**2.))
eul(2) = asin(2.*(q(4)*q(2)-q(3)*q(1)))
eul(3) = atan2(2.*(q(4)*q(1)+q(2)*q(3)), &
	1.-2.*(q(1)**2.+q(2)**2.))
end subroutine quat2eul

! ======== INDEX FUNCTION ========
subroutine indexfunction(s, z, L)
use initialization
implicit none
real:: s, z(m, nn), L(nn)
integer:: k
k=int((s-T_0)*m/(T_1-T_0))+1
L=z(k, :)
end subroutine indexfunction

! ======== OPTIMAL CONTROL ========
subroutine optimalcontrol(prec, mask, state, cost)
use initialization
implicit none
external integrand
integer:: i, j, k, mask(N)
real:: deriv(N), midpoint(N), r(nn), y(m, nn)
real:: convergence, prec, state(m+1, N), hh
real:: cost_prev, cost
real:: H(m, N), y_0, G_aux(m, N), G_auxx(m, N)
real:: G(m, N), integ(N),norm, aux(m, N), tam, eta, F

open(11,file='results_converg', &
status='replace',action='write')

hh = (T_1-T_0)/m
y_0 = 1.
y = 1.
eta = 1.e-01;

! // Optimal control algorithm ========
do

convergence=0.
do i=1, m
	deriv=(state(i+1, :)-state(i, :))/hh
	midpoint=0.5*(state(i, :)+state(i+1, :))
	call &
	constraints(T_0+hh*(i-0.5), midpoint, deriv, r)
	do k=1, nn
		if (convergence<abs(r(k)*y(i, k))) then
			convergence=abs(r(k)*y(i, k))
		endif
	end do
end do
print*, 'convergence ', convergence
write(11,*) convergence
if (convergence<prec) exit

cost_prev=1.e+10

do
	cost=0.
	integ=0.
	do i=1, m
		midpoint=0.5*(state(i,:)+state(i+1,:))
		deriv=(state(i+1,:)-state(i,:))/hh
		call auxintegrand1(T_0+hh*(i-0.5), &
		midpoint,deriv,y,H(i,:))
		call integrand(T_0+hh*(i-0.5), &
		midpoint,deriv,y,F)
		cost=cost+hh*F
		call auxintegrand2(T_0+hh*(i-0.5), &
		midpoint,deriv,y,T_1,G_aux(i,:))
		call auxintegrand2(T_0+hh*(i-0.5), &
		midpoint,deriv,y,T_0,G_auxx(i,:))
		do j=1, N
			if (mask(j).eq.1) then
				G(i,j)=G_aux(i,j)
				integ(j)=integ(j)+hh*G(i,j)
			else
				G(i,j)=G_auxx(i,j)
			end if
		end do
	end do
	cost_prev=cost
	norm=0.
	do i=1, m
		aux(i,:)=0.
		do k=1, i
			do j=1, N
				if (mask(j).eq.1) then
					aux(i,j)=&
					aux(i,j)+H(k,j)
				else
					aux(i,:)=&
					aux(i,:)-hh*G(k,:)
				end if
			end do
		end do
		do j=1, N
			if (mask(j).eq.1) then
				aux(i,j)=&
				aux(i,j)*hh*(hh*i-(T_1-T_0))
			end if
		end do
		do k=i+1, m
			do j=1, N
				if (mask(j).eq.1) then
					aux(i,j)=&
					aux(i,j)+hh*G(k,j)
				else
					aux(i,:)=aux(i,:)-&
					hh**2*i*H(k,:)
				end if
			end do
		end do
		aux(i,:)=aux(i,:)+&
		(hh*i-(T_1-T_0))*integ/(T_1-T_0)
		norm=norm+dot_product(aux(i,:), aux(i,:))
	end do
	tam=eta
	do
		if (abs(tam)<1.e-05) exit
		state(2:m+1,:)=state(2:m+1,:)+tam*aux
		cost=0.
		do i=1, m
			midpoint=&
			0.5*(state(i,:)+state(i+1,:))
			deriv=(state(i+1,:)-state(i,:))/hh
			call integrand(T_0+hh*(i-0.5), &
			midpoint,deriv,y,F)
			cost=cost+hh*F
		end do
		if (cost-cost_prev<0.) exit
		state(2:m+1,:)=state(2:m+1,:)-tam*aux
		tam=(0.5*norm*tam**2)/&
		(cost-cost_prev+norm*tam)
	end do
	if (abs(tam)<prec) exit
end do
s
do i=1, m
	deriv=(state(i+1,:)-state(i,:))/hh
	midpoint=0.5*(state(i,:)+state(i+1,:))
	call constraints(T_0+hh*(i-0.5),midpoint,deriv,r)
	do k=1, nn
		y(i,k)=y(i,k)*exp(y(i, k)*r(k))
	end do
end do
if (y_0/maxval(y)<1.e-05) exit
end do
! ======== Optimal control algorithm //
close(12)
end subroutine optimalcontrol


      PROGRAM xsvdvar
C     driver for routine svdvar
      INTEGER NCVM,MA,MP
      PARAMETER(MP=6,MA=3,NCVM=MA)
      INTEGER i,j
      REAL v(MP,MP),w(MP),cvm(NCVM,NCVM),tru(MA,MA)
      DATA w/0.0,1.0,2.0,3.0,4.0,5.0/
      DATA v/1.0,2.0,3.0,4.0,5.0,6.0,1.0,2.0,3.0,4.0,5.0,6.0,
     *     1.0,2.0,3.0,4.0,5.0,6.0,1.0,2.0,3.0,4.0,5.0,6.0,
     *     1.0,2.0,3.0,4.0,5.0,6.0,1.0,2.0,3.0,4.0,5.0,6.0/
      DATA tru/1.25,2.5,3.75,2.5,5.0,7.5,3.75,7.5,11.25/
      write(*,'(/1x,a)') 'Matrix V'
      do 11 i=1,MP
        write(*,'(1x,6f12.6)') (v(i,j),j=1,MP)
11    continue
      write(*,'(/1x,a)') 'Vector W'
      write(*,'(1x,6f12.6)') (w(i),i=1,MP)
      call svdvar(v,MA,MP,w,cvm,NCVM)
      write(*,'(/1x,a)') 'Covariance matrix from SVDVAR'
      do 12 i=1,MA
        write(*,'(1x,3f12.6)') (cvm(i,j),j=1,MA)
12    continue
      write(*,'(/1x,a)') 'Expected covariance matrix'
      do 13 i=1,MA
        write(*,'(1x,3f12.6)') (tru(i,j),j=1,MA)
13    continue
      END

NR-Functions/Numerical Recipes- Example & Functions/Examples/xsvdvar.for


program problem_13

!! What is the ID of the earliest bus you can take to
!! the airport multiplied by the number of minutes you'll
!! need to wait for that bus?

use aoc_utilities
use iso_fortran_env, ip => int32

implicit none

integer,parameter :: chunk_size = 256

integer :: n, iunit, istat, i, j, best_bus, bus
character(len=:),allocatable :: line
logical :: status_ok
integer(ip) :: earliest_timestamp, timestamp, best_timestamp, wait_time
type(string),dimension(:),allocatable :: vals

open(newunit=iunit, file='input.txt', iostat=istat)

call read_line_from_file(iunit,chunk_size,line,status_ok)
read(line,'(I10)') earliest_timestamp

call read_line_from_file(iunit,chunk_size,line,status_ok)
call split(line,',',chunk_size,vals)

n = size(vals)
best_bus = 0                 ! closest so far to earliest_timestamp
best_timestamp = huge(1_ip)  !

do i = 1, n
    if (vals(i)%str == 'x') cycle
    read(vals(i)%str, '(I10)') bus

    timestamp = 0
    do
        timestamp = timestamp + bus
        if (timestamp >= earliest_timestamp) then
            write(*,*) 'for bus ', bus, ' : ', timestamp
            if (timestamp<best_timestamp) then
                best_timestamp = timestamp
                best_bus = bus
            end if
            exit
        end if
    end do

end do

wait_time = best_timestamp - earliest_timestamp

write(*,*) ''
write(*,*) 'best bus:       ', best_bus
write(*,*) 'best timestamp: ', best_timestamp
write(*,*) 'wait time:      ', best_timestamp - earliest_timestamp
write(*,*) 'ID * wait time: ', best_bus * wait_time

end program problem_13


SUBROUTINE fftsg3d ( n, zout )
  INTEGER, PARAMETER :: dp = SELECTED_REAL_KIND ( 14, 200 )
  INTEGER, DIMENSION(*), INTENT(IN)                    :: n
  COMPLEX(KIND=dp), DIMENSION(*), INTENT(INOUT)        :: zout
  INTEGER                                              :: nx
  nx = n ( 1 )
  CALL mltfftsg ( zout, nx, nx )
END SUBROUTINE fftsg3d


program main

    implicit none

    real(kind=8), dimension(6) :: v

    v = (/1, 2, 3, 5, 6, 7/)

! Plot results to screen
    write (*, '(a, f10.7)') 'Self-written routine: ', norm(v)
    write (*, '(a, 3x, f10.7)') 'Intrinsic routine: ', norm2(v)
    write (*, '(a, 10x, f10.7)') 'Difference: ', norm2(v) - norm(v)

contains

    ! Calculate the euclidean norm of an n-dimensional vector
    pure function norm(vector) result(res)

        real(kind=8), dimension(:), intent(in) :: vector
        real(kind=8)                           :: res
        integer                                :: i

        res = 0
        do i = 1, size(vector), 2
            res = res + vector(i)**2
        end do

        res = sqrt(res)

    end function norm

end program main


c     ============================================
      subroutine setaux(maxmx,maxmy,mbc,mx,my,xlower,ylower,dx,dy,
     &                  maux,aux)
c     ============================================
c
c     # set auxiliary arrays
c     # variable coefficient acoustics
c     #  aux(i,j,1) = impedance Z in (i,j) cell
c     #  aux(i,j,2) = sound speed c in (i,j) cell
c
c     # Piecewise constant medium with single interface at x=0.5
c     # Density and sound speed to left and right are set in setprob.f

c
c
      implicit double precision (a-h,o-z)
      dimension aux(1-mbc:maxmx+mbc,1-mbc:maxmy+mbc, 2)
      common /comaux/ zl,cl,zr,cr
c
c     # density and bulk moduli:
      rhol = zl/cl
      rhor = zr/cr
      bulkl = cl**2 * rhol
      bulkr = cr**2 * rhor

      do 30 j=1-mbc,my+mbc
       do 20 i=1-mbc,mx+mbc
          xl = xlower + (i-1.0d0)*dx
          yl = ylower + (j-1.0d0)*dy

c         # determine what fraction wl of this cell lies to the
c         # "left" of the interface:
	  call cellave(xl,yl,dx,dy,wl)
	  wr = 1.d0 - wl

c         # average density:
	  rho = wl*rhol + wr*rhor

c         # harmonic average bulk modulus:
	  bulk = 1.d0 / (wl/bulkl + wr/bulkr)

c         # average sound speed:
	  aux(i,j,2) = dsqrt(bulk/rho)

c         # average impedance:
	  aux(i,j,1) = rho*aux(i,j,2)

   20     continue
   30    continue

       return
       end


program benchmark_maptran
!! Benchmark speed of array
use, intrinsic:: iso_fortran_env, only: int64, real64
use maptran, only: wp, geodetic2ecef

implicit none (type, external)

integer(int64) :: tic, toc, rate
real(real64) :: time
integer :: N, i
real(wp), allocatable, dimension(:,:,:) :: lat, lon, alt, x, y, z
character(8) :: buf

N = 100
call get_command_argument(1, buf, status=i)
if (i==0) read(buf,*) N
allocate(lat(N,N,N))
allocate(lon, alt, x, y, z, mold=lat)

call random_number(lat)
call random_number(lon)
call random_number(alt)

call system_clock(count_rate=rate) !< set timer to highest precision

call system_clock(tic)
call geodetic2ecef(lat, lon, alt, x, y, z)
call system_clock(toc)

time = (toc-tic) / real(rate, wp)

print '(F7.1,A,I8,A,I4)',time*1000,' ms for ',N,'^3 array geodetic2ecef,  real bits:',storage_size(z)

end program


MODULE nrtype
! Definition of types taken from Numerical Recipes
  INTEGER, PARAMETER :: DP = SELECTED_REAL_KIND(12,100)
  REAL(DP), PARAMETER :: PI=3.141592653589793238462643383279502884197_dp
  REAL(DP), PARAMETER :: PIO2=1.57079632679489661923132169163975144209858_dp
  REAL(DP), PARAMETER :: TWOPI=6.283185307179586476925286766559005768394_dp
  REAL(DP), PARAMETER :: SQRT2=1.41421356237309504880168872420969807856967_dp
  REAL(DP), PARAMETER :: EULER=0.5772156649015328606065120900824024310422_dp
  REAL(DP), PARAMETER :: ONE = 1,  ZERO = 0
END MODULE nrtype


module interpolate_mod
  implicit none
  save
contains

  subroutine interpolate(wl,array,val)
    use search_bisec_mod
    implicit none

    real*8, intent(in) :: wl
    real*8, intent(in) :: array(:,:)

    real*8, intent(out) :: val
    integer :: l,u

    ! search by bisection- get x bounds
    call search_bisec(wl,array(1,:),l,u)

    !use interpolation to find corresponding y
    val=(wl-array(1,l))/(array(1,u)-array(1,l))
    val=(array(2,u)-array(2,l))*val
    val=array(2,l) + val

    return
  end subroutine interpolate
end module interpolate_mod


  FUNCTION InitSquare( Model, n, t ) RESULT(f)
    USE Types
    USE SolverUtils
    IMPLICIT NONE

    TYPE(Model_t) :: Model
    LOGICAL :: Visited = .FALSE., GotIt
    INTEGER :: n,i,j
    REAL(KIND=dp) :: t,x,y,r,Lx,Ly,dist(4),f,a1,a2,b1,b2

    SAVE Visited, a1, a2, b1, b2

    IF(.NOT. Visited) THEN
      Lx = 1.0
      Ly = 1.0

      a1 = 0.25 * Lx
      a2 = 0.75 * Lx
      b1 = 0.25 * Ly
      b2 = 0.75 * Ly
      Visited = .TRUE.
    END IF

    x = Model % Nodes % x(n)
    y = Model % Nodes % y(n)

    ! distance from horizontal lines
    IF( x < a1 ) THEN
      dist(1) = SQRT( (x-a1)**2.0 + (y-b1)**2.0)
      dist(2) = SQRT( (x-a1)**2.0 + (y-b2)**2.0)
    ELSE IF( x > a2 ) THEN
      dist(1) = SQRT( (x-a2)**2.0 + (y-b1)**2.0)
      dist(2) = SQRT( (x-a2)**2.0 + (y-b2)**2.0)
    ELSE
      dist(1) = ABS(y - b1)
      dist(2) = ABS(y - b2)
    END IF

    ! distance from vertical lines
    IF( y < b1 ) THEN
      dist(3) = SQRT( (x-a1)**2.0 + (y-b1)**2.0)
      dist(4) = SQRT( (x-a2)**2.0 + (y-b1)**2.0)
    ELSE IF( y > b2 ) THEN
      dist(3) = SQRT( (x-a1)**2.0 + (y-b2)**2.0)
      dist(4) = SQRT( (x-a2)**2.0 + (y-b2)**2.0)
    ELSE
      dist(3) = ABS(x - a1)
      dist(4) = ABS(x - a2)
    END IF

    ! minimum distance to the sides of the rectangle
    f = MINVAL( ABS( dist ) )

    ! set negative sign inside the rectangle
    IF( .NOT. (x > a1 .AND. x < a2 .AND. y > b1 .AND. y < b2) ) THEN
      f = -f
    END IF

  END FUNCTION InitSquare


! A reference is https://www.nag.com/nagware/np/r70_doc/ieee_arithmetic.html
program test_ieee_exceptions
use :: ieee_exceptions, only: ieee_set_halting_mode, ieee_invalid, &
       ieee_overflow, ieee_underflow, ieee_divide_by_zero, ieee_inexact
implicit none
real :: a, b, c, d, e, f, g
logical, parameter :: halt_program = .true.
call ieee_set_halting_mode([ieee_divide_by_zero, ieee_underflow, &
     ieee_overflow, ieee_invalid],halting=halt_program)
! causes halt if divide_by_zero, underflow, overflow, or NaN conditions occur
! remove element from array constructor to have program continue with that condition
f = 1.0
print*,"computing 1.0/0.0"
print*,f/0.0
e = tiny(e)
print*,"tiny =",e,", computing tiny/10"
print*,e/10
d = huge(d)
print*,"huge =",d,", computing huge**2"
print*,d**2
a = 0.0
b = 0.0
print*,"a, b=",a,b
print*,"computing c = a/b"
c = a/b
print*,"c=",c
! since floating point roundoff is ubiquitous, probably
! do not want the following mode
call ieee_set_halting_mode(ieee_inexact, halting=halt_program)
print*,"computing 2.0/3.0"
g = 2.0/3.0
print*,"2.0/3.0 =",g
end program test_ieee_exceptions
! output on Windows with ifort -traceback ieee_exceptions.f90
!  computing 1.0/0.0
! forrtl: error (73): floating divide by zero
! Image              PC                Routine            Line        Source
! ieee_exceptions.e  00007FF6EBBF112A  MAIN__                     14  ieee_exceptions.f90


module random

use types, only: dp
use utils, only: stop_error
implicit none
private
public randn, rand_gamma

interface randn
    module procedure randn_scalar
    module procedure randn_vector
    module procedure randn_matrix
    module procedure randn_vector_n
end interface

interface rand_gamma
    module procedure rand_gamma_scalar
    module procedure rand_gamma_vector
    module procedure rand_gamma_matrix
    module procedure rand_gamma_vector_n
end interface

contains

subroutine randn_scalar(x)
! Returns a psuedorandom scalar drawn from the standard normal distribution.
!
! [1] Marsaglia, G., & Bray, T. A. (1964). A Convenient Method for Generating
!       Normal Variables. SIAM Review, 6(3), 260–264.
real(dp), intent(out) :: x
logical, save :: first = .true.
real(dp), save :: u(2)
real(dp) :: r2
if (first) then
    do
        call random_number(u)
        u = 2*u-1
        r2 = sum(u**2)
        if (r2 < 1 .and. r2 > 0) exit
    end do
    u = u * sqrt(-2*log(r2)/r2)
    x = u(1)
else
    x = u(2)
end if
first = .not. first
end subroutine

subroutine randn_vector_n(n, x)
integer, intent(in) :: n
real(dp), intent(out) :: x(n)
integer :: i
do i = 1, size(x)
    call randn(x(i))
end do
end subroutine

subroutine randn_vector(x)
real(dp), intent(out) :: x(:)
call randn_vector_n(size(x), x)
end subroutine

subroutine randn_matrix(x)
real(dp), intent(out) :: x(:, :)
call randn_vector_n(size(x), x)
end subroutine

subroutine rand_gamma0(a, first, fn_val)
! Returns a psuedorandom scalar drawn from the gamma distribution.
!
! The shape parameter a >= 1.
!
! [1] Marsaglia, G., & Tsang, W. W. (2000). A Simple Method for Generating
! Gamma Variables. ACM Transactions on Mathematical Software (TOMS), 26(3),
! 363–372.
real(dp), intent(in) :: a
logical, intent(in) :: first
real(dp), intent(out) :: fn_val
real(dp), save :: c, d
real(dp) :: U, v, x
if (a < 1) call stop_error("Shape parameter must be >= 1")
if (first) then
    d = a - 1._dp/3
    c = 1/sqrt(9*d)
end if
do
    do
        call randn(x)
        v = (1 + c*x)**3
        if (v > 0) exit
    end do
    call random_number(U)
    ! Note: the number 0.0331 below is exact, see [1].
    if (U < 1 - 0.0331_dp*x**4) then
        fn_val = d*v
        exit
    else if (log(U) < x**2/2 + d*(1 - v + log(v))) then
        fn_val = d*v
        exit
    end if
end do
end subroutine

subroutine rand_gamma_scalar(a, x)
real(dp), intent(in) :: a
real(dp), intent(out) :: x
call rand_gamma0(a, .true., x)
end subroutine

subroutine rand_gamma_vector_n(a, n, x)
real(dp), intent(in) :: a
integer, intent(in) :: n
real(dp), intent(out) :: x(n)
integer :: i
call rand_gamma0(a, .true., x(1))
do i = 2, size(x)
    call rand_gamma0(a, .false., x(i))
end do
end subroutine

subroutine rand_gamma_vector(a, x)
real(dp), intent(in) :: a
real(dp), intent(out) :: x(:)
call rand_gamma_vector_n(a, size(x), x)
end subroutine

subroutine rand_gamma_matrix(a, x)
real(dp), intent(in) :: a
real(dp), intent(out) :: x(:, :)
call rand_gamma_vector_n(a, size(x), x)
end subroutine

end module


      SUBROUTINE asolve(n,b,x,itrnsp)
      INTEGER n,itrnsp,ija,NMAX,i
      DOUBLE PRECISION x(n),b(n),sa
      PARAMETER (NMAX=1000)
      COMMON /mat/ sa(NMAX),ija(NMAX)
      do 11 i=1,n
        x(i)=b(i)/sa(i)
11    continue
      return
      END

NR-Functions/Numerical Recipes- Example & Functions/Functions/asolve.for