      SUBROUTINE MB03BD( JOB, DEFL, COMPQ, QIND, K, N, H, ILO, IHI, S,
     $                   A, LDA1, LDA2, Q, LDQ1, LDQ2, ALPHAR, ALPHAI,
     $                   BETA, SCAL, IWORK, LIWORK, DWORK, LDWORK,
     $                   IWARN, INFO )
C
C     SLICOT RELEASE 5.5.
C
C     Copyright (c) 2002-2012 NICONET e.V.
C
C     PURPOSE
C
C     To find the eigenvalues of the generalized matrix product
C
C                  S(1)           S(2)                 S(K)
C          A(:,:,1)     * A(:,:,2)     * ... * A(:,:,K)
C
C     where A(:,:,H) is upper Hessenberg and A(:,:,i), i <> H, is upper
C     triangular, using a double-shift version of the periodic
C     QZ method. In addition, A may be reduced to periodic Schur form:
C     A(:,:,H) is upper quasi-triangular and all the other factors
C     A(:,:,I) are upper triangular. Optionally, the 2-by-2 triangular
C     matrices corresponding to 2-by-2 diagonal blocks in A(:,:,H)
C     are so reduced that their product is a 2-by-2 diagonal matrix.
C
C     If COMPQ = 'U' or COMPQ = 'I', then the orthogonal factors are
C     computed and stored in the array Q so that for S(I) = 1,
C
C                         T
C             Q(:,:,I)(in)   A(:,:,I)(in)   Q(:,:,MOD(I,K)+1)(in)
C                          T                                        (1)
C         =   Q(:,:,I)(out)  A(:,:,I)(out)  Q(:,:,MOD(I,K)+1)(out),
C
C     and for S(I) = -1,
C
C                                  T
C             Q(:,:,MOD(I,K)+1)(in)   A(:,:,I)(in)   Q(:,:,I)(in)
C                                   T                               (2)
C         =   Q(:,:,MOD(I,K)+1)(out)  A(:,:,I)(out)  Q(:,:,I)(out).
C
C     A partial generation of the orthogonal factors can be realized
C     via the array QIND.
C
C     ARGUMENTS
C
C     Mode Parameters
C
C     JOB     CHARACTER*1
C             Specifies the computation to be performed, as follows:
C             = 'E': compute the eigenvalues only; A will not
C                    necessarily be put into periodic Schur form;
C             = 'S': put A into periodic Schur form, and return the
C                    eigenvalues in ALPHAR, ALPHAI, BETA, and SCAL;
C             = 'T': as JOB = 'S', but A is put into standardized
C                    periodic Schur form, that is, the general product
C                    of the 2-by-2 triangular matrices corresponding to
C                    a complex eigenvalue is diagonal.
C
C     DEFL    CHARACTER*1
C             Specifies the deflation strategy to be used, as follows:
C             = 'C': apply a careful deflation strategy, that is,
C                    the criteria are based on the magnitudes of
C                    neighboring elements and infinite eigenvalues are
C                    only deflated at the top; this is the recommended
C                    option;
C             = 'A': apply a more aggressive strategy, that is,
C                    elements on the subdiagonal or diagonal are set
C                    to zero as soon as they become smaller in magnitude
C                    than eps times the norm of the corresponding
C                    factor; this option is only recommended if
C                    balancing is applied beforehand and convergence
C                    problems are observed.
C
C     COMPQ   CHARACTER*1
C             Specifies whether or not the orthogonal transformations
C             should be accumulated in the array Q, as follows:
C             = 'N': do not modify Q;
C             = 'U': modify (update) the array Q by the orthogonal
C                    transformations that are applied to the matrices in
C                    the array A to reduce them to periodic Schur form;
C             = 'I': like COMPQ = 'U', except that each matrix in the
C                    array Q will be first initialized to the identity
C                    matrix;
C             = 'P': use the parameters as encoded in QIND.
C
C     QIND    INTEGER array, dimension (K)
C             If COMPQ = 'P', then this array describes the generation
C             of the orthogonal factors as follows:
C                If QIND(I) > 0, then the array Q(:,:,QIND(I)) is
C             modified by the transformations corresponding to the
C             i-th orthogonal factor in (1) and (2).
C                If QIND(I) < 0, then the array Q(:,:,-QIND(I)) is
C             initialized to the identity and modified by the
C             transformations corresponding to the i-th orthogonal
C             factor in (1) and (2).
C                If QIND(I) = 0, then the transformations corresponding
C             to the i-th orthogonal factor in (1), (2) are not applied.
C
C     Input/Output Parameters
C
C     K       (input)  INTEGER
C             The number of factors.  K >= 1.
C
C     N       (input)  INTEGER
C             The order of each factor in the array A.  N >= 0.
C
C     H       (input)  INTEGER
C             Hessenberg index. The factor A(:,:,H) is on entry in upper
C             Hessenberg form.  1 <= H <= K.
C
C     ILO     (input)  INTEGER
C     IHI     (input)  INTEGER
C             It is assumed that each factor in A is already upper
C             triangular in rows and columns 1:ILO-1 and IHI+1:N.
C             1 <= ILO <= IHI <= N, if N > 0;
C             ILO = 1 and IHI  = 0, if N = 0.
C
C     S       (input)  INTEGER array, dimension (K)
C             The leading K elements of this array must contain the
C             signatures of the factors. Each entry in S must be either
C             1 or -1.
C
C     A       (input/output)  DOUBLE PRECISION array, dimension
C                             (LDA1,LDA2,K)
C             On entry, the leading N-by-N-by-K part of this array
C             must contain the factors in upper Hessenberg-triangular
C             form, that is, A(:,:,H) is upper Hessenberg and the other
C             factors are upper triangular.
C             On exit, if JOB = 'S' and INFO = 0, the leading
C             N-by-N-by-K part of this array contains the factors of
C             A in periodic Schur form, that is, A(:,:,H) is upper quasi
C             triangular and the other factors are upper triangular.
C             On exit, if JOB = 'T' and INFO = 0, the leading
C             N-by-N-by-K part of this array contains the factors of
C             A as for the option JOB = 'S', but the product of the
C             triangular factors corresponding to a 2-by-2 block in
C             A(:,:,H) is diagonal.
C             On exit, if JOB = 'E', then the leading N-by-N-by-K part
C             of this array contains meaningless elements.
C
C     LDA1    INTEGER
C             The first leading dimension of the array A.
C             LDA1 >= MAX(1,N).
C
C     LDA2    INTEGER
C             The second leading dimension of the array A.
C             LDA2 >= MAX(1,N).
C
C     Q       (input/output)  DOUBLE PRECISION array, dimension
C                             (LDQ1,LDQ2,K)
C             On entry, if COMPQ = 'U', the leading N-by-N-by-K part
C             of this array must contain the initial orthogonal factors
C             as described in (1) and (2).
C             On entry, if COMPQ = 'P', only parts of the leading
C             N-by-N-by-K part of this array must contain some
C             orthogonal factors as described by the parameters QIND.
C             If COMPQ = 'I', this array should not set on entry.
C             On exit, if COMPQ = 'U' or COMPQ = 'I', the leading
C             N-by-N-by-K part of this array contains the modified
C             orthogonal factors as described in (1) and (2).
C             On exit, if COMPQ = 'P', only parts of the leading
C             N-by-N-by-K part contain some modified orthogonal factors
C             as described by the parameters QIND.
C             This array is not referenced if COMPQ = 'N'.
C
C     LDQ1    INTEGER
C             The first leading dimension of the array Q.  LDQ1 >= 1,
C             and, if COMPQ <> 'N', LDQ1 >= MAX(1,N).
C
C     LDQ2    INTEGER
C             The second leading dimension of the array Q.  LDQ2 >= 1,
C             and, if COMPQ <> 'N', LDQ2 >= MAX(1,N).
C
C     ALPHAR  (output) DOUBLE PRECISION array, dimension (N)
C             On exit, if INFO = 0, the leading N elements of this array
C             contain the scaled real parts of the eigenvalues of the
C             matrix product A. The i-th eigenvalue of A is given by
C
C             (ALPHAR(I) + ALPHAI(I)*SQRT(-1))/BETA(I) * BASE**SCAL(I),
C
C             where BASE is the machine base (often 2.0).
C
C     ALPHAI  (output) DOUBLE PRECISION array, dimension (N)
C             On exit, if INFO = 0, the leading N elements of this array
C             contain the scaled imaginary parts of the eigenvalues
C             of A.
C
C     BETA    (output) DOUBLE PRECISION array, dimension (N)
C             On exit, if INFO = 0, the leading N elements of this array
C             contain indicators for infinite eigenvalues. That is, if
C             BETA(I) = 0.0, then the i-th eigenvalue is infinite.
C             Otherwise BETA(I) is set to 1.0.
C
C     SCAL    (output) INTEGER array, dimension (N)
C             On exit, if INFO = 0, the leading N elements of this array
C             contain the scaling parameters for the eigenvalues of A.
C
C
C     Workspace
C
C     IWORK   INTEGER array, dimension (LIWORK)
C             On exit, if INFO = 0, IWORK(1) returns the optimal LIWORK.
C             On exit, if INFO = -22, IWORK(1) returns the minimum value
C             of LIWORK.
C
C     LIWORK  INTEGER
C             The length of the array IWORK.  LIWORK  >= MAX( 1,2*K ).
C
C     DWORK   DOUBLE PRECISION array, dimension (LDWORK)
C             On exit, if INFO = 0, DWORK(1) returns the optimal LDWORK.
C             On exit, if INFO = -24, DWORK(1) returns the minimum value
C             of LDWORK.
C
C     LDWORK  INTEGER
C             The length of the array DWORK.
C             If DEFL = 'C', LDWORK >= MAX( 1,MAX( 2*N,8*K ) );
C             if DEFL = 'A', LDWORK >= MAX( 1,K + MAX( 2*N,8*K ) ).
C
C     Warning Indicator
C
C     IWARN   INTEGER
C             = 0        :  no warnings;
C             = 1,..,N-1 :  A is in periodic Schur form, but the
C                           algorithm was not able to reveal information
C                           about the eigenvalues from the 2-by-2
C                           blocks.
C                           ALPHAR(i), ALPHAI(i), BETA(i) and SCAL(i),
C                           can be incorrect for i = 1, ..., IWARN+1;
C             = N        :  some eigenvalues might be inaccurate.
C
C     Error Indicator
C
C     INFO    INTEGER
C             = 0      :  succesful exit;
C             < 0      :  if INFO = -i, the i-th argument had an illegal
C                         value;
C             = 1,..,N :  the periodic QZ iteration did not converge.
C                         A is not in periodic Schur form, but
C                         ALPHAR(i), ALPHAI(i), BETA(i) and SCAL(i), for
C                         i = INFO+1,...,N should be correct.
C
C     METHOD
C
C     A modified version of the periodic QZ algorithm is used [1], [2].
C
C     REFERENCES
C
C     [1] Bojanczyk, A., Golub, G. H. and Van Dooren, P.
C         The periodic Schur decomposition: algorithms and applications.
C         In F.T. Luk (editor), Advanced Signal Processing Algorithms,
C         Architectures, and Implementations III, Proc. SPIE Conference,
C         vol. 1770, pp. 31-42, 1992.
C
C     [2] Kressner, D.
C         An efficient and reliable implementation of the periodic QZ
C         algorithm. IFAC Workshop on Periodic Control Systems (PSYCO
C         2001), Como (Italy), August 27-28 2001. Periodic Control
C         Systems 2001 (IFAC Proceedings Volumes), Pergamon.
C
C     NUMERICAL ASPECTS
C
C     The implemented method is numerically backward stable.
C                                 3
C     The algorithm requires 0(K N ) floating point operations.
C
C     CONTRIBUTOR
C
C     D. Kressner, Technical Univ. Berlin, Germany, June 2001.
C
C     REVISIONS
C
C     V. Sima, Research Institute for Informatics, Bucharest, Romania,
C     July 2009, SLICOT Library version of the routine PHGEQZ.
C     V. Sima, June 2010, July 2010, Nov. 2010, Sep. 2011, Oct. 2011.
C
C     KEYWORDS
C
C     Eigenvalues, QZ algorithm, periodic QZ algorithm, orthogonal
C     transformation.
C
C     ******************************************************************
C
C     .. Parameters ..
      DOUBLE PRECISION  ZERO, ONE
      PARAMETER         ( ZERO = 0.0D+0, ONE = 1.0D+0 )
C     .. Scalar Arguments ..
      CHARACTER         COMPQ, DEFL, JOB
      INTEGER           H, IHI, ILO, INFO, IWARN, K, LDA1, LDA2, LDQ1,
     $                  LDQ2, LDWORK, LIWORK, N
C     .. Array Arguments ..
      INTEGER           IWORK(*), QIND(*), S(*), SCAL(*)
      DOUBLE PRECISION  A(LDA1,LDA2,*), ALPHAI(*), ALPHAR(*),
     $                  BETA(*), DWORK(*), Q(LDQ1,LDQ2,*)
C     .. Local Arrays ..
      INTEGER           ISEED(4)
      DOUBLE PRECISION  MACPAR(5)
C     .. Local Scalars ..
      LOGICAL           ADEFL, LCMPQ, LINIQ, LPARQ, LSCHR, LSVD
      INTEGER           AIND, I, IERR, IFIRST, IFRSTM, IITER, ILAST,
     $                  ILASTM, IN, J, J1, JDEF, JITER, JLO, L, LDEF,
     $                  LM, MAXIT, NTRA, OPTDW, OPTIW, QI, SINV, TITER,
     $                  ZITER
      DOUBLE PRECISION  BASE, CS, CS1, CS2, LGBAS, SAFMAX, SAFMIN,
     $                  SMLNUM, SN, SN1, SN2, TEMP, TEMP2, TEMP3, TOL,
     $                  ULP
C     .. Workspace Pointers ..
      INTEGER           MAPA, MAPQ, PDW, PFREE, PNORM
C     .. External Functions ..
      LOGICAL           LSAME
      DOUBLE PRECISION  DLAMCH, DLANHS
      EXTERNAL          DLAMCH, DLANHS, LSAME
C     .. External Subroutines ..
      EXTERNAL          DLABAD, DLARNV, DLARTG, DLASET, DROT, MA01BD,
     $                  MB03AD, MB03BA, MB03BB, MB03BC, MB03BE, XERBLA
C     .. Intrinsic Functions ..
      INTRINSIC         ABS, DBLE, INT, LOG, MAX, MIN, MOD
C
C     .. Executable Statements ..
C
C     Decode the scalar input parameters.
C
      LSVD  = LSAME( JOB,   'T' )
      LSCHR = LSAME( JOB,   'S' ) .OR. LSVD
      LINIQ = LSAME( COMPQ, 'I' )
      LCMPQ = LSAME( COMPQ, 'U' ) .OR. LINIQ
      LPARQ = LSAME( COMPQ, 'P' )
      ADEFL = LSAME( DEFL,  'A' )
      IWARN = 0
      IF ( ADEFL ) THEN
         OPTDW = MAX( 1, K + MAX( 2*N,8*K ) )
      ELSE
         OPTDW = MAX( 1, MAX( 2*N,8*K ) )
      END IF
      OPTIW = MAX( 1, 2*K )
C
C     Check the scalar input parameters.
C
      INFO = 0
      IF ( .NOT. ( LSCHR .OR. LSAME( JOB, 'E' ) ) ) THEN
         INFO = -1
      ELSE IF ( .NOT.( ADEFL .OR. LSAME( DEFL, 'C' ) ) ) THEN
         INFO = -2
      ELSE IF ( .NOT.( LCMPQ .OR. LPARQ .OR. LSAME( COMPQ, 'N' ) ) )
     $      THEN
         INFO = -3
      ELSE IF ( K.LT.0 ) THEN
         INFO = -5
      ELSE IF ( N.LT.0 ) THEN
         INFO = -6
      ELSE IF ( H.LT.1 .OR. H.GT.K ) THEN
         INFO = -7
      ELSE IF ( ILO.LT.1 ) THEN
         INFO = -8
      ELSE IF ( IHI.GT.N .OR. IHI.LT.ILO-1 ) THEN
         INFO = -9
      ELSE IF ( LDA1.LT.MAX( 1, N ) ) THEN
         INFO = -12
      ELSE IF ( LDA2.LT.MAX( 1, N ) ) THEN
         INFO = -13
      ELSE IF ( LDQ1.LT.1 .OR. ( ( LCMPQ .OR. LPARQ )
     $                             .AND. LDQ1.LT.N ) ) THEN
         INFO = -15
      ELSE IF ( LDQ2.LT.1 .OR. ( ( LCMPQ .OR. LPARQ )
     $                             .AND. LDQ2.LT.N ) ) THEN
         INFO = -16
      ELSE IF ( LIWORK.LT.OPTIW ) THEN
         IWORK(1) = OPTIW
         INFO = -22
      ELSE IF ( LDWORK.LT.OPTDW ) THEN
         DWORK(1) = DBLE( OPTDW )
         INFO = -24
      END IF
C
C     Return if there were illegal values.
C
      IF ( INFO.NE.0 ) THEN
         CALL XERBLA( 'MB03BD', -INFO )
         RETURN
      END IF
C
C     Quick return if possible.
C
      IF ( N.EQ.0 ) THEN
         DWORK(1) = ONE
         IWORK(1) = 1
         RETURN
      END IF
C
C     Initialize Q.
C
      DO 10  I = 1, K
         J = 0
         IF ( LINIQ ) THEN
            J = I
         ELSE IF ( LPARQ ) THEN
            J = -QIND(I)
         END IF
         IF ( J.NE.0 )
     $      CALL DLASET( 'Full', N, N, ZERO, ONE, Q(1,1,J), LDQ1 )
   10 CONTINUE
C
C     Compute Maps for accessing A and Q.
C
      MAPA = 0
      MAPQ = K
      QI   = 0
      CALL MB03BA( K, H, S, SINV, IWORK(MAPA+1), IWORK(MAPQ+1) )
C
C     Machine Constants.
C
      IN = IHI + 1 - ILO
      SAFMIN = DLAMCH( 'SafeMinimum' )
      SAFMAX = ONE / SAFMIN
      ULP    = DLAMCH( 'Precision' )
      CALL DLABAD( SAFMIN, SAFMAX )
      SMLNUM = SAFMIN*( IN / ULP )
      BASE   = DLAMCH( 'Base' )
      LGBAS  = LOG( BASE )
C
      MACPAR(2) = DLAMCH( 'Underflow' )
      IF ( LSVD ) THEN
         MACPAR(1) = DLAMCH( 'ORmax' )
         MACPAR(3) = SAFMIN
         MACPAR(4) = DLAMCH( 'Epsilon' )
         MACPAR(5) = BASE
      END IF
      IF ( K.GE.INT( LOG( MACPAR(2) ) / LOG( ULP ) ) ) THEN
C
C        Start Iteration with a controlled zero shift.
C
         ZITER = -1
      ELSE
         ZITER = 0
      END IF
C
C     Compute norms.
C
      IF ( ADEFL ) THEN
         PNORM = 0
         PFREE = K
         DO 20  I = 1, K
            AIND = IWORK( MAPA + I )
            DWORK(I) = DLANHS( 'Frobenius', IN, A(ILO,ILO,AIND), LDA1,
     $                         DWORK( PFREE + 1 ) )
   20    CONTINUE
      ELSE
         PFREE = 0
      END IF
C
C     Set Eigenvalues IHI+1:N.
C
      DO 30  J = IHI + 1, N
         CALL MA01BD( BASE, LGBAS, K, S, A(J,J,1), LDA1*LDA2, ALPHAR(J),
     $                BETA(J), SCAL(J) )
         ALPHAI(J) = ZERO
   30 CONTINUE
C
C     If IHI < ILO, skip QZ steps.
C
      IF ( IHI.LT.ILO )
     $   GO TO 500
C
C     MAIN PERIODIC QZ ITERATION LOOP.
C
C     Initialize dynamic indices.
C
C     Eigenvalues ILAST+1:N have been found.
C        Column operations modify rows IFRSTM:whatever.
C        Row operations modify columns whatever:ILASTM.
C
C     If only eigenvalues are being computed, then
C        IFRSTM is the row of the last splitting row above row ILAST;
C        this is always at least ILO.
C     IITER counts iterations since the last eigenvalue was found,
C        to tell when to use an observed zero or random shift.
C     MAXIT is the maximum number of QZ sweeps allowed.
C
      ILAST = IHI
      IF ( LSCHR ) THEN
         IFRSTM = 1
         ILASTM = N
      ELSE
         IFRSTM = ILO
         ILASTM = IHI
      END IF
      IITER = 0
      TITER = 0
      ISEED(1) = 1
      ISEED(2) = 0
      ISEED(3) = 0
      ISEED(4) = 1
      MAXIT = 30 * IN
C
      DO  490 JITER = 1, MAXIT
C
C        Special Case: ILAST = ILO.
C
         IF ( ILAST.EQ.ILO )
     $      GO TO 390
C
C        **************************************************************
C        *                     CHECK FOR DEFLATION                    *
C        **************************************************************
C
C        Test 1:  Deflation in the Hessenberg matrix.
C
         IF ( ADEFL )
     $      TOL = MAX( SAFMIN, DWORK( PNORM + 1 )*ULP )
         AIND = IWORK( MAPA + 1 )
         JLO = ILO
         DO 40  J = ILAST, ILO + 1, -1
            IF ( .NOT.ADEFL ) THEN
               TOL = ABS( A(J-1,J-1,AIND) ) + ABS( A(J,J,AIND) )
               IF ( TOL.EQ.ZERO )
     $             TOL = DLANHS( '1', J-ILO+1, A(ILO,ILO,AIND), LDA1,
     $                           DWORK(PFREE+1) )
                TOL = MAX( ULP*TOL, SMLNUM )
            END IF
            IF ( ABS( A(J,J-1,AIND) ).LE.TOL ) THEN
               A(J,J-1,AIND) = ZERO
               JLO = J
               IF ( J.EQ.ILAST )
     $            GO TO 390
               GO TO 50
            END IF
   40    CONTINUE
C
   50    CONTINUE
C
C        Test 2:  Deflation in the triangular matrices with index 1.
C
         DO 70  LDEF = 2, K
            AIND = IWORK( MAPA + LDEF )
            IF ( S(AIND).EQ.SINV ) THEN
               IF ( ADEFL )
     $            TOL = MAX( SAFMIN, DWORK( PNORM + LDEF )*ULP )
               DO 60  J = ILAST, JLO, -1
                  IF ( .NOT.ADEFL ) THEN
                     IF ( J.EQ.ILAST ) THEN
                        TOL = ABS( A(J-1,J,AIND) )
                     ELSE IF ( J.EQ.JLO ) THEN
                        TOL = ABS( A(J,J+1,AIND) )
                     ELSE
                        TOL = ABS( A(J-1,J,AIND) )
     $                      + ABS( A(J,J+1,AIND) )
                     END IF
                     IF ( TOL.EQ.ZERO )
     $                  TOL = DLANHS( '1', J-JLO+1, A(JLO,JLO,AIND),
     $                                LDA1, DWORK(PFREE+1) )
                     TOL = MAX( ULP*TOL, SMLNUM )
                  END IF
                  IF ( ABS( A(J,J,AIND) ).LE.TOL ) THEN
                     A(J,J,AIND) = ZERO
                     GO TO 170
                  END IF
   60          CONTINUE
            END IF
   70    CONTINUE
C
C        Test 3:  Deflation in the triangular matrices with index -1.
C
         DO 90  LDEF = 2, K
            AIND = IWORK( MAPA + LDEF )
            IF ( S(AIND).NE.SINV ) THEN
               IF ( ADEFL )
     $            TOL = MAX( SAFMIN, DWORK( PNORM + LDEF )*ULP )
               DO 80  J = ILAST, JLO, -1
                  IF ( .NOT.ADEFL ) THEN
                     IF ( J.EQ.ILAST ) THEN
                        TOL = ABS( A(J-1,J,AIND) )
                     ELSE IF ( J.EQ.JLO ) THEN
                        TOL = ABS( A(J,J+1,AIND) )
                     ELSE
                        TOL = ABS( A(J-1,J,AIND) )
     $                      + ABS( A(J,J+1,AIND) )
                     END IF
                     IF ( TOL.EQ.ZERO )
     $                  TOL = DLANHS( '1', J-JLO+1, A(JLO,JLO,AIND),
     $                                LDA1, DWORK(PFREE+1) )
                     TOL = MAX( ULP*TOL, SMLNUM )
                  END IF
                  IF ( ABS( A(J,J,AIND) ).LE.TOL ) THEN
                     A(J,J,AIND) = ZERO
                     GO TO 320
                  END IF
   80          CONTINUE
            END IF
   90    CONTINUE
C
C        Test 4:  Controlled zero shift.
C
         IF ( ZITER.GE.7 .OR. ZITER.LT.0 ) THEN
C
C           Make Hessenberg matrix upper triangular.
C
            AIND = IWORK( MAPA + 1 )
            PDW  = PFREE + 1
            DO 100 J = JLO, ILAST - 1
               TEMP = A(J,J,AIND)
               CALL DLARTG( TEMP, A(J+1,J,AIND), CS, SN, A(J,J,AIND) )
               A(J+1,J,AIND) = ZERO
               CALL DROT( ILASTM-J, A(J,J+1,AIND), LDA1,
     $                    A(J+1,J+1,AIND), LDA1, CS, SN )
               DWORK(PDW)   = CS
               DWORK(PDW+1) = SN
               PDW = PDW + 2
  100       CONTINUE
            IF ( LCMPQ ) THEN
               QI = IWORK( MAPQ + 1 )
            ELSE IF ( LPARQ ) THEN
               QI = ABS( QIND(IWORK( MAPQ + 1 )) )
            END IF
            IF ( QI.NE.0 ) THEN
               PDW = PFREE + 1
               DO 110  J = JLO, ILAST - 1
                  CS = DWORK(PDW)
                  SN = DWORK(PDW+1)
                  PDW = PDW + 2
                  CALL DROT( N, Q(1,J,QI), 1, Q(1,J+1,QI), 1, CS, SN )
  110          CONTINUE
            END IF
C
C           Propagate Transformations back to A_1.
C
            DO 150  L = K, 2, -1
               AIND = IWORK( MAPA + L )
               PDW  = PFREE + 1
               IF ( ADEFL )
     $            TOL = MAX( SAFMIN, DWORK( PNORM + L )*ULP )
               IF ( S(AIND).EQ.SINV ) THEN
                  DO 120  J = JLO, ILAST - 1
                     CS = DWORK(PDW)
                     SN = DWORK(PDW+1)
                     IF ( SN.NE.ZERO ) THEN
                        CALL DROT( J+2-IFRSTM, A(IFRSTM,J,AIND), 1,
     $                             A(IFRSTM,J+1,AIND), 1, CS, SN )
C
C                       Check for deflation.
C
                        IF ( .NOT.ADEFL ) THEN
                           TOL = ABS( A(J,J,AIND) ) +
     $                           ABS( A(J+1,J+1,AIND) )
                           IF ( TOL.EQ.ZERO )
     $                        TOL = DLANHS( '1', J-JLO+2,
     $                                      A(JLO,JLO,AIND), LDA1,
     $                                      DWORK(PFREE+2*N+1) )
                           TOL = MAX( ULP*TOL, SMLNUM )
                           IF ( ABS( A(J+1,J,AIND) ).LE.TOL ) THEN
                              CS = ONE
                              SN = ZERO
                              A(J+1,J,AIND) = ZERO
                           END IF
                        END IF
                     END IF
                     IF ( SN.NE.ZERO ) THEN
                        TEMP = A(J,J,AIND)
                        CALL DLARTG( TEMP, A(J+1,J,AIND), CS, SN,
     $                               A(J,J,AIND) )
                        A(J+1,J,AIND) = ZERO
                        CALL DROT( ILASTM-J, A(J,J+1,AIND), LDA1,
     $                             A(J+1,J+1,AIND), LDA1, CS, SN )
                     END IF
                     DWORK(PDW)   = CS
                     DWORK(PDW+1) = SN
                     PDW = PDW + 2
  120             CONTINUE
               ELSE
                  DO 130  J = JLO, ILAST - 1
                     CS = DWORK(PDW)
                     SN = DWORK(PDW+1)
                     IF ( SN.NE.ZERO ) THEN
                        CALL DROT( ILASTM-J+1, A(J,J,AIND), LDA1,
     $                             A(J+1,J,AIND), LDA1, CS, SN )
C
C                       Check for deflation.
C
                        IF ( .NOT.ADEFL ) THEN
                           TOL = ABS( A(J,J,AIND) ) +
     $                           ABS( A(J+1,J+1,AIND) )
                           IF ( TOL.EQ.ZERO )
     $                        TOL = DLANHS( '1', J-JLO+2,
     $                                      A(JLO,JLO,AIND), LDA1,
     $                                      DWORK(PFREE+2*N+1) )
                           TOL = MAX( ULP*TOL, SMLNUM )
                           IF ( ABS( A(J+1,J,AIND) ).LE.TOL ) THEN
                              CS = ONE
                              SN = ZERO
                              A(J+1,J,AIND) = ZERO
                           END IF
                        END IF
                     END IF
                     IF ( SN.NE.ZERO ) THEN
                        TEMP = A(J+1,J+1,AIND)
                        CALL DLARTG( TEMP, A(J+1,J,AIND), CS, SN,
     $                               A(J+1,J+1,AIND) )
                        A(J+1,J,AIND) = ZERO
                        SN = -SN
                        CALL DROT( J+1-IFRSTM, A(IFRSTM,J,AIND), 1,
     $                             A(IFRSTM,J+1,AIND), 1, CS, SN )
                     END IF
                     DWORK(PDW)   = CS
                     DWORK(PDW+1) = SN
                     PDW = PDW + 2
  130             CONTINUE
               END IF
               IF ( LCMPQ ) THEN
                  QI = IWORK( MAPQ + L )
               ELSE IF ( LPARQ ) THEN
                  QI = ABS( QIND(IWORK( MAPQ + L )) )
               END IF
               IF ( QI.NE.0 ) THEN
                  PDW = PFREE + 1
                  DO 140  J = JLO, ILAST - 1
                     CS  = DWORK(PDW)
                     SN  = DWORK(PDW+1)
                     PDW = PDW + 2
                     IF ( SN.NE.ZERO )
     $                  CALL DROT( N, Q(1,J,QI), 1, Q(1,J+1,QI), 1, CS,
     $                             SN )
  140             CONTINUE
               END IF
  150       CONTINUE
C
C           Apply the transformations to the right hand side of the
C           Hessenberg factor.
C
            AIND = IWORK( MAPA + 1 )
            PDW  = PFREE + 1
            ZITER = 0
            DO 160  J = JLO, ILAST - 1
               CS = DWORK(PDW)
               SN = DWORK(PDW+1)
               IF ( SN.NE.ZERO ) THEN
                  CALL DROT( J+2-IFRSTM, A(IFRSTM,J,AIND), 1,
     $                       A(IFRSTM,J+1,AIND), 1, CS, SN )
               ELSE
                  ZITER = -1
               END IF
               PDW = PDW + 2
  160       CONTINUE
C
C           No QZ iteration.
C
            GO TO 480
         END IF
C
C        **************************************************************
C        *                     HANDLE DEFLATIONS                      *
C        **************************************************************
C
C        Case I: Deflation occurs in the Hessenberg matrix. The QZ
C                iteration is only applied to the JLO:ILAST part.
C
         IFIRST = JLO
C
C        Go to the periodic QZ steps.
C
         GO TO 400
C
C        Case II: Deflation occurs in a triangular matrix with index 1.
C
C        Do an unshifted periodic QZ step.
C
  170    CONTINUE
         JDEF = J
         AIND = IWORK( MAPA + 1 )
         PDW  = PFREE + 1
         DO 180  J = JLO, JDEF - 1
            TEMP = A(J,J,AIND)
            CALL DLARTG( TEMP, A(J+1,J,AIND), CS, SN, A(J,J,AIND) )
            A(J+1,J,AIND) = ZERO
            CALL DROT( ILASTM-J, A(J,J+1,AIND), LDA1, A(J+1,J+1,AIND),
     $                 LDA1, CS, SN )
            DWORK(PDW)   = CS
            DWORK(PDW+1) = SN
            PDW = PDW + 2
  180    CONTINUE
         IF ( LCMPQ ) THEN
            QI = IWORK( MAPQ + 1 )
         ELSE IF ( LPARQ ) THEN
            QI = ABS( QIND(IWORK( MAPQ + 1 )) )
         END IF
         IF ( QI.NE.0 ) THEN
            PDW = PFREE + 1
            DO 190  J = JLO, JDEF - 1
               CS  = DWORK(PDW)
               SN  = DWORK(PDW+1)
               PDW = PDW + 2
               CALL DROT( N, Q(1,J,QI), 1, Q(1,J+1,QI), 1, CS, SN )
  190       CONTINUE
         END IF
C
C        Propagate the transformations through the triangular matrices.
C        Due to the zero element on the diagonal of the LDEF-th factor,
C        the number of transformations drops by one.
C
         DO 230  L = K, 2, -1
            AIND = IWORK( MAPA + L )
            IF ( L.LT.LDEF ) THEN
               NTRA = JDEF - 2
            ELSE
               NTRA = JDEF - 1
            END IF
            PDW = PFREE + 1
            IF ( S(AIND).EQ.SINV ) THEN
               DO 200  J = JLO, NTRA
                  CS = DWORK(PDW)
                  SN = DWORK(PDW+1)
                  CALL DROT( J+2-IFRSTM, A(IFRSTM,J,AIND), 1,
     $                       A(IFRSTM,J+1,AIND), 1, CS, SN )
                  TEMP = A(J,J,AIND)
                  CALL DLARTG( TEMP, A(J+1,J,AIND), CS, SN,
     $                         A(J,J,AIND) )
                  A(J+1,J,AIND) = ZERO
                  CALL DROT( ILASTM-J, A(J,J+1,AIND), LDA1,
     $                       A(J+1,J+1,AIND), LDA1, CS, SN )
                  DWORK(PDW)   = CS
                  DWORK(PDW+1) = SN
                  PDW = PDW + 2
  200          CONTINUE
            ELSE
               DO 210  J = JLO, NTRA
                  CS = DWORK(PDW)
                  SN = DWORK(PDW+1)
                  CALL DROT( ILASTM-J+1, A(J,J,AIND), LDA1,
     $                       A(J+1,J,AIND), LDA1, CS, SN )
                  TEMP = A(J+1,J+1,AIND)
                  CALL DLARTG( TEMP, A(J+1,J,AIND), CS, SN,
     $                         A(J+1,J+1,AIND) )
                  A(J+1,J,AIND) = ZERO
                  SN = -SN
                  CALL DROT( J+1-IFRSTM, A(IFRSTM,J,AIND), 1,
     $                       A(IFRSTM,J+1,AIND), 1, CS, SN )
                  DWORK(PDW)   = CS
                  DWORK(PDW+1) = SN
                  PDW = PDW + 2
  210          CONTINUE
            END IF
            IF ( LCMPQ ) THEN
               QI = IWORK( MAPQ + L )
            ELSE IF ( LPARQ ) THEN
               QI = ABS( QIND(IWORK( MAPQ + L )) )
            END IF
            IF ( QI.NE.0 ) THEN
               PDW = PFREE + 1
               DO 220  J = JLO, NTRA
                  CS  = DWORK(PDW)
                  SN  = DWORK(PDW+1)
                  PDW = PDW + 2
                  CALL DROT( N, Q(1,J,QI), 1, Q(1,J+1,QI), 1, CS, SN )
  220          CONTINUE
            END IF
  230    CONTINUE
C
C        Apply the transformations to the right hand side of the
C        Hessenberg factor.
C
         AIND = IWORK( MAPA + 1 )
         PDW = PFREE + 1
         DO 240  J = JLO, JDEF - 2
            CS = DWORK(PDW)
            SN = DWORK(PDW+1)
            CALL DROT( J+2-IFRSTM, A(IFRSTM,J,AIND), 1,
     $                 A(IFRSTM,J+1,AIND), 1, CS, SN )
            PDW = PDW + 2
  240    CONTINUE
C
C        Do an unshifted periodic QZ step.
C
         PDW  = PFREE + 1
         DO 250  J = ILAST, JDEF + 1, -1
            TEMP = A(J,J,AIND)
            CALL DLARTG( TEMP, A(J,J-1,AIND), CS, SN, A(J,J,AIND) )
            A(J,J-1,AIND) = ZERO
            SN = -SN
            CALL DROT( J-IFRSTM, A(IFRSTM,J-1,AIND), 1,
     $                 A(IFRSTM,J,AIND), 1, CS, SN )
            DWORK(PDW)   = CS
            DWORK(PDW+1) = SN
            PDW = PDW + 2
  250    CONTINUE
         IF ( LCMPQ ) THEN
            QI = IWORK( MAPQ + 2 )
         ELSE IF ( LPARQ ) THEN
            QI = ABS( QIND(IWORK( MAPQ + 2 )) )
         END IF
         IF ( QI.NE.0 ) THEN
            PDW = PFREE + 1
            DO 260  J = ILAST, JDEF + 1, -1
               CS  = DWORK(PDW)
               SN  = DWORK(PDW+1)
               PDW = PDW + 2
               CALL DROT( N, Q(1,J-1,QI), 1, Q(1,J,QI), 1, CS, SN )
  260       CONTINUE
         END IF
C
C        Propagate the transformations through the triangular matrices.
C
         DO 300  L = 2, K
            AIND = IWORK( MAPA + L )
            IF ( L.GT.LDEF ) THEN
               NTRA = JDEF + 2
            ELSE
               NTRA = JDEF + 1
            END IF
            PDW = PFREE + 1
            IF ( S(AIND).NE.SINV ) THEN
               DO 270  J = ILAST, NTRA, -1
                  CS = DWORK(PDW)
                  SN = DWORK(PDW+1)
                  CALL DROT( J+1-IFRSTM, A(IFRSTM,J-1,AIND), 1,
     $                       A(IFRSTM,J,AIND), 1, CS, SN )
                  TEMP = A(J-1,J-1,AIND)
                  CALL DLARTG( TEMP, A(J,J-1,AIND), CS, SN,
     $                         A(J-1,J-1,AIND) )
                  A(J,J-1,AIND) = ZERO
                  CALL DROT( ILASTM-J+1, A(J-1,J,AIND), LDA1,
     $                       A(J,J,AIND), LDA1, CS, SN )
                  DWORK(PDW)   = CS
                  DWORK(PDW+1) = SN
                  PDW = PDW + 2
  270          CONTINUE
            ELSE
               DO 280  J = ILAST, NTRA, -1
                  CS = DWORK(PDW)
                  SN = DWORK(PDW+1)
                  CALL DROT( ILASTM-J+2, A(J-1,J-1,AIND), LDA1,
     $                       A(J,J-1,AIND), LDA1, CS, SN )
                  TEMP = A(J,J,AIND)
                  CALL DLARTG( TEMP, A(J,J-1,AIND), CS, SN,
     $                         A(J,J,AIND) )
                  A(J,J-1,AIND) = ZERO
                  SN = -SN
                  CALL DROT( J-IFRSTM, A(IFRSTM,J-1,AIND), 1,
     $                       A(IFRSTM,J,AIND), 1, CS, SN )
                  DWORK(PDW)   = CS
                  DWORK(PDW+1) = SN
                  PDW = PDW + 2
  280          CONTINUE
            END IF
            LM = L + 1
            IF ( L.EQ.K )
     $         LM = 1
            IF ( LCMPQ ) THEN
               QI = IWORK( MAPQ + LM )
            ELSE IF ( LPARQ ) THEN
               QI = ABS( QIND(IWORK( MAPQ + LM )) )
            END IF
            IF ( QI.NE.0 ) THEN
               PDW = PFREE + 1
               DO 290  J = ILAST, NTRA, -1
                  CS  = DWORK(PDW)
                  SN  = DWORK(PDW+1)
                  PDW = PDW + 2
                  CALL DROT( N, Q(1,J-1,QI), 1, Q(1,J,QI), 1, CS, SN )
  290          CONTINUE
            END IF
  300    CONTINUE
C
C        Apply the transformations to the left hand side of the
C        Hessenberg factor.
C
         AIND = IWORK( MAPA + 1 )
         PDW  = PFREE + 1
         DO 310  J = ILAST, JDEF + 2, -1
            CS  = DWORK(PDW)
            SN  = DWORK(PDW+1)
            PDW = PDW + 2
            CALL DROT( ILASTM-J+2, A(J-1,J-1,AIND), LDA1, A(J,J-1,AIND),
     $                 LDA1, CS, SN )
  310    CONTINUE
C
C        No QZ iteration.
C
         GO TO 480
C
C        Case III: Deflation occurs in a triangular matrix with
C                  index -1.
C
  320    CONTINUE
         JDEF = J
         PDW  = PFREE + 1
         IF ( JDEF.GT.( ( ILAST - JLO + 1 )/2 ) ) THEN
C
C           Chase the zero downwards to the last position.
C
            DO 340  J1 = JDEF, ILAST - 1
               J = J1
               AIND = IWORK( MAPA + LDEF )
               TEMP = A(J,J+1,AIND)
               CALL DLARTG( TEMP, A(J+1,J+1,AIND), CS, SN,
     $                      A(J,J+1,AIND) )
               A(J+1,J+1,AIND) = ZERO
               CALL DROT( ILASTM-J-1, A(J,J+2,AIND), LDA1,
     $                    A(J+1,J+2,AIND), LDA1, CS, SN )
               LM = LDEF + 1
               IF ( LDEF.EQ.K )
     $            LM = 1
               IF ( LCMPQ ) THEN
                  QI = IWORK( MAPQ + LM )
               ELSE IF ( LPARQ ) THEN
                  QI = ABS( QIND(IWORK( MAPQ + LM )) )
               END IF
               IF ( QI.NE.0 )
     $            CALL DROT( N, Q(1,J,QI), 1, Q(1,J+1,QI), 1, CS, SN )
               DO 330  L = 1, K - 1
                  AIND = IWORK( MAPA + LM )
                  IF ( LM.EQ.1 ) THEN
                     CALL DROT( ILASTM-J+2, A(J,J-1,AIND), LDA1,
     $                          A(J+1,J-1,AIND), LDA1, CS, SN )
                     TEMP = A(J+1,J,AIND)
                     CALL DLARTG( TEMP, A(J+1,J-1,AIND), CS, SN,
     $                            A(J+1,J,AIND) )
                     A(J+1,J-1,AIND) = ZERO
                     SN = -SN
                     CALL DROT( J-IFRSTM+1, A(IFRSTM,J-1,AIND), 1,
     $                          A(IFRSTM,J,AIND), 1, CS, SN )
                     J = J-1
                  ELSE IF ( S(AIND).EQ.SINV ) THEN
                     CALL DROT( ILASTM-J+1, A(J,J,AIND), LDA1,
     $                          A(J+1,J,AIND), LDA1, CS, SN )
                     TEMP = A(J+1,J+1,AIND)
                     CALL DLARTG( TEMP, A(J+1,J,AIND), CS, SN,
     $                            A(J+1,J+1,AIND) )
                     A(J+1,J,AIND) = ZERO
                     SN = -SN
                     CALL DROT( J-IFRSTM+1, A(IFRSTM,J,AIND), 1,
     $                          A(IFRSTM,J+1,AIND), 1, CS, SN )
                  ELSE
                     CALL DROT( J-IFRSTM+2, A(IFRSTM,J,AIND), 1,
     $                          A(IFRSTM,J+1,AIND), 1, CS, SN )
                     TEMP = A(J,J,AIND)
                     CALL DLARTG( TEMP, A(J+1,J,AIND), CS, SN,
     $                            A(J,J,AIND) )
                     A(J+1,J,AIND) = ZERO
                     CALL DROT( ILASTM-J, A(J,J+1,AIND), LDA1,
     $                          A(J+1,J+1,AIND), LDA1, CS, SN )
                  END IF
                  LM = LM + 1
                  IF ( LM.GT.K )
     $               LM = 1
                  IF ( LCMPQ ) THEN
                     QI = IWORK( MAPQ + LM )
                  ELSE IF ( LPARQ ) THEN
                     QI = ABS( QIND(IWORK( MAPQ + LM )) )
                  END IF
                  IF ( QI.NE.0 )
     $               CALL DROT( N, Q(1,J,QI), 1, Q(1,J+1,QI), 1, CS,
     $                          SN )
  330          CONTINUE
               AIND = IWORK( MAPA + LDEF )
               CALL DROT( J-IFRSTM+1, A(IFRSTM,J,AIND), 1,
     $                    A(IFRSTM,J+1,AIND), 1, CS, SN )
  340       CONTINUE
C
C           Deflate the last element in the Hessenberg matrix.
C
            AIND = IWORK( MAPA + 1 )
            J = ILAST
            TEMP = A(J,J,AIND)
            CALL DLARTG( TEMP, A(J,J-1,AIND), CS, SN, A(J,J,AIND) )
            A(J,J-1,AIND) = ZERO
            SN = -SN
            CALL DROT( J-IFRSTM, A(IFRSTM,J-1,AIND), 1,
     $                 A(IFRSTM,J,AIND), 1, CS, SN )
            IF ( LCMPQ ) THEN
               QI = IWORK( MAPQ + 2 )
            ELSE IF ( LPARQ ) THEN
               QI = ABS( QIND(IWORK( MAPQ + 2 )) )
            END IF
            IF ( QI.NE.0 )
     $         CALL DROT( N, Q(1,J-1,QI), 1, Q(1,J,QI), 1, CS, SN )
            DO 350  L = 2, LDEF - 1
               AIND = IWORK( MAPA + L )
               IF ( S(AIND).NE.SINV ) THEN
                  CALL DROT( J+1-IFRSTM, A(IFRSTM,J-1,AIND), 1,
     $                       A(IFRSTM,J,AIND), 1, CS, SN )
                  TEMP = A(J-1,J-1,AIND)
                  CALL DLARTG( TEMP, A(J,J-1,AIND), CS, SN,
     $                         A(J-1,J-1,AIND) )
                  A(J,J-1,AIND) = ZERO
                  CALL DROT( ILASTM-J+1, A(J-1,J,AIND), LDA1,
     $                       A(J,J,AIND), LDA1, CS, SN )
               ELSE
                  CALL DROT( ILASTM-J+2, A(J-1,J-1,AIND), LDA1,
     $                       A(J,J-1,AIND), LDA1, CS, SN )
                  TEMP = A(J,J,AIND)
                  CALL DLARTG( TEMP, A(J,J-1,AIND), CS, SN,
     $                         A(J,J,AIND) )
                  A(J,J-1,AIND) = ZERO
                  SN = -SN
                  CALL DROT( J-IFRSTM, A(IFRSTM,J-1,AIND), 1,
     $                       A(IFRSTM,J,AIND), 1, CS, SN )
               END IF
               LM = L + 1
               IF ( L.EQ.K )
     $            LM = 1
               IF ( LCMPQ ) THEN
                  QI = IWORK( MAPQ + LM )
               ELSE IF ( LPARQ ) THEN
                  QI = ABS( QIND(IWORK( MAPQ + LM )) )
               END IF
               IF ( QI.NE.0 )
     $            CALL DROT( N, Q(1,J-1,QI), 1, Q(1,J,QI), 1, CS, SN )
  350       CONTINUE
            AIND = IWORK( MAPA + LDEF )
            CALL DROT( J+1-IFRSTM, A(IFRSTM,J-1,AIND), 1,
     $                 A(IFRSTM,J,AIND), 1, CS, SN )
         ELSE
C
C           Chase the zero upwards to the first position.
C
            DO 370  J1 = JDEF, JLO + 1, -1
               J = J1
               AIND = IWORK( MAPA + LDEF )
               TEMP = A(J-1,J,AIND)
               CALL DLARTG( TEMP, A(J-1,J-1,AIND), CS, SN,
     $                      A(J-1,J,AIND) )
               A(J-1,J-1,AIND) = ZERO
               SN = -SN
               CALL DROT( J-IFRSTM-1, A(IFRSTM,J-1,AIND), 1,
     $                    A(IFRSTM,J,AIND), 1, CS, SN )
               IF ( LCMPQ ) THEN
                  QI = IWORK( MAPQ + LDEF )
               ELSE IF ( LPARQ ) THEN
                  QI = ABS( QIND(IWORK( MAPQ + LDEF )) )
               END IF
               IF ( QI.NE.0 )
     $            CALL DROT( N, Q(1,J-1,QI), 1, Q(1,J,QI), 1, CS, SN )
               LM = LDEF - 1
               DO 360  L = 1, K - 1
                  AIND = IWORK( MAPA + LM )
                  IF ( LM.EQ.1 ) THEN
                     CALL DROT( J-IFRSTM+2, A(IFRSTM,J-1,AIND), 1,
     $                          A(IFRSTM,J,AIND), 1, CS, SN )
                     TEMP = A(J,J-1,AIND)
                     CALL DLARTG( TEMP, A(J+1,J-1,AIND), CS, SN,
     $                            A(J,J-1,AIND) )
                     A(J+1,J-1,AIND) = ZERO
                     CALL DROT( ILASTM-J+1, A(J,J,AIND), LDA1,
     $                          A(J+1,J,AIND), LDA1, CS, SN )
                     J = J + 1
                  ELSE IF ( S(AIND).NE.SINV ) THEN
                     CALL DROT( ILASTM-J+2, A(J-1,J-1,AIND), LDA1,
     $                          A(J,J-1,AIND), LDA1, CS, SN )
                     TEMP = A(J,J,AIND)
                     CALL DLARTG( TEMP, A(J,J-1,AIND), CS, SN,
     $                            A(J,J,AIND) )
                     A(J,J-1,AIND) = ZERO
                     SN = -SN
                     CALL DROT( J-IFRSTM, A(IFRSTM,J-1,AIND), 1,
     $                          A(IFRSTM,J,AIND), 1, CS, SN )
                  ELSE
                     CALL DROT( J-IFRSTM+1, A(IFRSTM,J-1,AIND), 1,
     $                          A(IFRSTM,J,AIND), 1, CS, SN )
                     TEMP = A(J-1,J-1,AIND)
                     CALL DLARTG( TEMP, A(J,J-1,AIND), CS, SN,
     $                            A(J-1,J-1,AIND) )
                     A(J,J-1,AIND) = ZERO
                     CALL DROT( ILASTM-J+1, A(J-1,J,AIND), LDA1,
     $                          A(J,J,AIND), LDA1, CS, SN )
                  END IF
                  IF ( LCMPQ ) THEN
                     QI = IWORK( MAPQ + LM )
                  ELSE IF ( LPARQ ) THEN
                     QI = ABS( QIND(IWORK( MAPQ + LM )) )
                  END IF
                  IF ( QI.NE.0 )
     $               CALL DROT( N, Q(1,J-1,QI), 1, Q(1,J,QI), 1, CS,
     $                          SN )
                  LM = LM - 1
                  IF ( LM.LE.0 )
     $               LM = K
  360          CONTINUE
               AIND = IWORK( MAPA + LDEF )
               CALL DROT( ILASTM-J+1, A(J-1,J,AIND), LDA1, A(J,J,AIND),
     $                    LDA1, CS, SN )
  370       CONTINUE
C
C           Deflate the first element in the Hessenberg matrix.
C
            AIND = IWORK( MAPA + 1 )
            J = JLO
            TEMP = A(J,J,AIND)
            CALL DLARTG( TEMP, A(J+1,J,AIND), CS, SN, A(J,J,AIND) )
            A(J+1,J,AIND) = ZERO
            CALL DROT( ILASTM-J, A(J,J+1,AIND), LDA1, A(J+1,J+1,AIND),
     $                 LDA1, CS, SN )
            IF ( LCMPQ ) THEN
               QI = IWORK( MAPQ + 1 )
            ELSE IF ( LPARQ ) THEN
               QI = ABS( QIND(IWORK( MAPQ + 1 )) )
            END IF
            IF ( QI.NE.0 )
     $         CALL DROT( N, Q(1,J,QI), 1, Q(1,J+1,QI), 1, CS, SN )
            DO 380  L = K, LDEF + 1, -1
               AIND = IWORK( MAPA + L )
               IF ( S(AIND).EQ.SINV ) THEN
                  CALL DROT( J+2-IFRSTM, A(IFRSTM,J,AIND), 1,
     $                       A(IFRSTM,J+1,AIND), 1, CS, SN )
                  TEMP = A(J,J,AIND)
                  CALL DLARTG( TEMP, A(J+1,J,AIND), CS, SN,
     $                         A(J,J,AIND) )
                  A(J+1,J,AIND) = ZERO
                  CALL DROT( ILASTM-J, A(J,J+1,AIND), LDA1,
     $                       A(J+1,J+1,AIND), LDA1, CS, SN )
               ELSE
                  CALL DROT( ILASTM-J+1, A(J,J,AIND), LDA1,
     $                       A(J+1,J,AIND), LDA1, CS, SN )
                  TEMP = A(J+1,J+1,AIND)
                  CALL DLARTG( TEMP, A(J+1,J,AIND), CS, SN,
     $                         A(J+1,J+1,AIND) )
                  A(J+1,J,AIND) = ZERO
                  SN = -SN
                  CALL DROT( J+1-IFRSTM, A(IFRSTM,J,AIND), 1,
     $                       A(IFRSTM,J+1,AIND), 1, CS, SN )
               END IF
               IF ( LCMPQ ) THEN
                  QI = IWORK( MAPQ + L )
               ELSE IF ( LPARQ ) THEN
                  QI = ABS( QIND(IWORK( MAPQ + L )) )
               END IF
               IF ( QI.NE.0 )
     $            CALL DROT( N, Q(1,J,QI), 1, Q(1,J+1,QI), 1, CS, SN )
  380       CONTINUE
            AIND = IWORK( MAPA + LDEF )
            CALL DROT( ILASTM-J, A(J,J+1,AIND), LDA1, A(J+1,J+1,AIND),
     $                 LDA1, CS, SN )
         END IF
C
C        No QZ iteration.
C
         GO TO 480
C
C        Special case: A 1x1 block splits off at the bottom.
C
  390    CONTINUE
         CALL MA01BD( BASE, LGBAS, K, S, A(ILAST,ILAST,1), LDA1*LDA2,
     $                ALPHAR(ILAST), BETA(ILAST), SCAL(ILAST) )
         ALPHAI(ILAST) = ZERO
C
C        Go to next block - exit if finished.
C
         ILAST = ILAST - 1
         IF ( ILAST.LT.ILO )
     $      GO TO 500
C
C        Reset iteration counters.
C
         IITER = 0
         TITER = 0
         IF ( ZITER.NE.-1 )
     $      ZITER = 0
         IF ( .NOT.LSCHR ) THEN
            ILASTM = ILAST
            IF ( IFRSTM.GT.ILAST )
     $         IFRSTM = ILO
         END IF
C
C        No QZ iteration.
C
         GO TO 480
C
C        **************************************************************
C        *                      PERIODIC QZ STEP                      *
C        **************************************************************
C
C        It is assumed that IFIRST < ILAST.
C
  400    CONTINUE
C
         IITER = IITER + 1
         ZITER = ZITER + 1
         IF( .NOT.LSCHR )
     $      IFRSTM = IFIRST
         IF ( IFIRST+1.EQ.ILAST ) THEN
C
C           Special case -- 2x2 block.
C
            J = ILAST - 1
            IF ( TITER.LT.2 ) THEN
               TITER = TITER + 1
C
C              Try to deflate the 2-by-2 problem.
C
               PDW = PFREE + 1
               DO 410  L = 1, K
                  DWORK( PDW   ) = A(J,J,L)
                  DWORK( PDW+1 ) = A(J+1,J,L)
                  DWORK( PDW+2 ) = A(J,J+1,L)
                  DWORK( PDW+3 ) = A(J+1,J+1,L)
                  PDW = PDW + 4
  410          CONTINUE
               CALL MB03BE( K, IWORK(MAPA+1), S, SINV, DWORK(PFREE+1),
     $                      2, 2 )
               IF ( ABS( DWORK(PFREE+2) ).LT.ULP*(
     $              MAX( ABS( DWORK(PFREE+1) ),
     $                   ABS( DWORK(PFREE+3) ),
     $                   ABS( DWORK(PFREE+4) ) ) ) ) THEN
C
C                 Construct a perfect shift polynomial. This may fail,
C                 so we try it twice (indicated by TITER).
C
                  CS1 = ONE
                  SN1 = ONE
                  DO 420  L = K, 2, -1
                     AIND = IWORK(MAPA+L)
                     TEMP = DWORK( PFREE + AIND*4 )
                     IF ( S(AIND).EQ.SINV ) THEN
                        CALL DLARTG( CS1*A(J,J,AIND), SN1*TEMP, CS1,
     $                               SN1, TEMP )
                     ELSE
                        CALL DLARTG( CS1*TEMP, SN1*A(J,J,AIND), CS1,
     $                               SN1, TEMP )
                     END IF
  420             CONTINUE
                  AIND = IWORK(MAPA+1)
                  TEMP = DWORK( PFREE + AIND*4 )
                  CALL DLARTG( A(J,J,AIND)*CS1-TEMP*SN1,
     $                         A(J+1,J,AIND)*CS1, CS1, SN1, TEMP )
                  GO TO 460
               END IF
            END IF
C
C           Looks like a complex block.
C           1. Compute the product SVD of the triangular matrices
C             (optionally).
C
            IF ( LSVD ) THEN
               CALL MB03BC( K, IWORK(MAPA+1), S, SINV, A(J,J,1), LDA1,
     $                      LDA2, MACPAR, DWORK(PFREE+1),
     $                      DWORK(PFREE+K+1), DWORK(PFREE+2*K+1) )
C
C              Update factors and transformations.
C
               AIND = IWORK(MAPA+1)
               CS2  = DWORK(PFREE + 1)
               SN2  = DWORK(PFREE + K + 1)
               CALL DROT( ILASTM-IFRSTM+1, A(IFRSTM,J,AIND), 1,
     $                    A(IFRSTM,J+1,AIND), 1, CS2, SN2 )
               DO 430  L = 2, K
                  AIND = IWORK(MAPA+L)
                  IF ( LCMPQ ) THEN
                     QI = IWORK( MAPQ + L )
                  ELSE IF ( LPARQ ) THEN
                     QI = ABS( QIND(IWORK( MAPQ + L )) )
                  END IF
                  IF ( QI.NE.0 )
     $               CALL DROT( N, Q(1,J,QI), 1, Q(1,J+1,QI), 1, CS2,
     $                          SN2 )
                  CS1 = CS2
                  SN1 = SN2
                  CS2 = DWORK(PFREE + L)
                  SN2 = DWORK(PFREE + K + L)
                  IF (S(AIND).EQ.SINV) THEN
                     CALL DROT( ILASTM-J-1, A(J,J+2,AIND), LDA1,
     $                          A(J+1,J+2,AIND), LDA1, CS1, SN1 )
                     CALL DROT( J-IFRSTM, A(IFRSTM,J,AIND), 1,
     $                          A(IFRSTM,J+1,AIND), 1, CS2, SN2 )
                  ELSE
                     CALL DROT( ILASTM-J-1, A(J,J+2,AIND), LDA1,
     $                          A(J+1,J+2,AIND), LDA1, CS2, SN2 )
                     CALL DROT( J-IFRSTM, A(IFRSTM,J,AIND), 1,
     $                          A(IFRSTM,J+1,AIND), 1, CS1, SN1 )
                  END IF
  430          CONTINUE
               IF ( LCMPQ ) THEN
                  QI = IWORK( MAPQ + 1 )
               ELSE IF ( LPARQ ) THEN
                  QI = ABS( QIND(IWORK( MAPQ + 1 )) )
               END IF
               IF ( QI.NE.0 )
     $            CALL DROT( N, Q(1,J,QI), 1, Q(1,J+1,QI), 1, CS2, SN2 )
               AIND = IWORK(MAPA+1)
               CALL DROT( ILASTM-J+1, A(J,J,AIND), LDA1,
     $                    A(J+1,J,AIND), LDA1, CS2, SN2 )
            END IF
C
C           2. Compute complex eigenvalues.
C
            CALL MB03BB( BASE, LGBAS, ULP, K, IWORK(MAPA+1), S, SINV,
     $                   A(J,J,1), LDA1, LDA2, ALPHAR(J), ALPHAI(J),
     $                   BETA(J), SCAL(J), DWORK(PFREE + 1), IERR )
            IF ( IERR.EQ.1 ) THEN
C
C              The single shift periodic QZ did not converge, set
C              IWARN = J to indicate that the eigenvalues are not
C              assigned.
C
               IWARN = MAX( J, IWARN )
            ELSE IF ( IERR.EQ.2 ) THEN
C
C              Some computed eigenvalues might be inaccurate.
C
               IF ( IWARN.EQ.0 )
     $            IWARN = N
            END IF
C
C           Go to next block and reset counters.
C
            ILAST = IFIRST - 1
            IF ( ILAST.LT.ILO )
     $         GO TO 500
            IITER = 0
            TITER = 0
            IF ( ZITER.NE.-1 )
     $         ZITER = 0
            IF ( .NOT.LSCHR ) THEN
               ILASTM = ILAST
               IF ( IFRSTM.GT.ILAST )
     $           IFRSTM = ILO
            END IF
            GO TO 480
         END IF
C
C        Now, it is assumed that ILAST-IFIRST+1 >= 3.
C        Complex double shift.
C
         IF ( MOD( IITER, 30 ).EQ.0 ) THEN
C
C           Exceptional shift.
C
            CALL DLARNV( 3, ISEED, 1, TEMP  )
            CALL DLARNV( 3, ISEED, 1, TEMP2 )
            CALL DLARTG( TEMP, TEMP2, CS1, SN1, TEMP3 )
            CALL DLARNV( 3, ISEED, 1, TEMP  )
            CALL DLARNV( 3, ISEED, 1, TEMP2 )
            CALL DLARTG( TEMP, TEMP2, CS2, SN2, TEMP3 )
         ELSE
            CALL MB03AD( 'Double', K, ILAST-IFIRST+1, IWORK( MAPA + 1 ),
     $                   S, SINV, A(IFIRST,IFIRST,1), LDA1, LDA2, CS1,
     $                   SN1, CS2, SN2 )
         END IF
C
C        Do the sweeps.
C
         DO 450  J1 = IFIRST - 1, ILAST - 3
            J = J1 + 1
            AIND = IWORK( MAPA + 1 )
            IF ( LCMPQ ) THEN
               QI = IWORK( MAPQ + 1 )
            ELSE IF ( LPARQ ) THEN
               QI = ABS( QIND(IWORK( MAPQ + 1 )) )
            END IF
C
C           Create a bulge if J1 = IFIRST - 1, otherwise chase the
C           bulge.
C
            IF ( J1.LT.IFIRST ) THEN
               CALL DROT( ILASTM-J+1, A(J+1,J,AIND), LDA1,
     $                    A(J+2,J,AIND), LDA1, CS2, SN2 )
               CALL DROT( ILASTM-J+1, A(J,J,AIND), LDA1,
     $                    A(J+1,J,AIND), LDA1, CS1, SN1 )
            ELSE
               TEMP = A(J+1,J-1,AIND)
               CALL DLARTG( TEMP, A(J+2,J-1,AIND), CS2, SN2,
     $                      TEMP2 )
               TEMP = A(J,J-1,AIND)
               CALL DLARTG( TEMP, TEMP2, CS1, SN1, A(J,J-1,AIND) )
               A(J+1,J-1,AIND) = ZERO
               A(J+2,J-1,AIND) = ZERO
               CALL DROT( ILASTM-J+1, A(J+1,J,AIND), LDA1,
     $                    A(J+2,J,AIND), LDA1, CS2, SN2 )
               CALL DROT( ILASTM-J+1, A(J,J,AIND), LDA1,
     $                    A(J+1,J,AIND), LDA1, CS1, SN1 )
            END IF
            IF ( QI.NE.0 ) THEN
               CALL DROT( N, Q(1,J+1,QI), 1, Q(1,J+2,QI), 1, CS2, SN2 )
               CALL DROT( N, Q(1,J,  QI), 1, Q(1,J+1,QI), 1, CS1, SN1 )
            END IF
C
C           Propagate information from the right to A_1.
C
            DO 440  L = K, 2, -1
               AIND = IWORK( MAPA + L )
               IF ( S(AIND).EQ.SINV ) THEN
                  CALL DROT( J+3-IFRSTM, A(IFRSTM,J+1,AIND), 1,
     $                       A(IFRSTM,J+2,AIND), 1, CS2, SN2 )
                  TEMP = A(J+1,J+1,AIND)
                  CALL DLARTG( TEMP, A(J+2,J+1,AIND), CS2, SN2,
     $                         A(J+1,J+1,AIND) )
                  A(J+2,J+1,AIND) = ZERO
                  CALL DROT( ILASTM-J-1, A(J+1,J+2,AIND), LDA1,
     $                       A(J+2,J+2,AIND), LDA1, CS2, SN2 )
                  CALL DROT( J+2-IFRSTM, A(IFRSTM,J,AIND), 1,
     $                       A(IFRSTM,J+1,AIND), 1, CS1, SN1 )
                  TEMP = A(J,J,AIND)
                  CALL DLARTG( TEMP, A(J+1,J,AIND), CS1, SN1,
     $                         A(J,J,AIND) )
                  A(J+1,J,AIND) = ZERO
                  CALL DROT( ILASTM-J, A(J,J+1,AIND), LDA1,
     $                       A(J+1,J+1,AIND), LDA1, CS1, SN1 )
               ELSE
                  CALL DROT( ILASTM-J+1, A(J+1,J,AIND), LDA1,
     $                       A(J+2,J,AIND), LDA1, CS2, SN2 )
                  TEMP = A(J+2,J+2,AIND)
                  CALL DLARTG( TEMP, A(J+2,J+1,AIND), CS2, SN2,
     $                         A(J+2,J+2,AIND) )
                  A(J+2,J+1,AIND) = ZERO
                  SN2 = -SN2
                  CALL DROT( J+2-IFRSTM, A(IFRSTM,J+1,AIND), 1,
     $                       A(IFRSTM,J+2,AIND), 1, CS2, SN2 )
                  CALL DROT( ILASTM-J+1, A(J,J,AIND), LDA1,
     $                       A(J+1,J,AIND), LDA1, CS1, SN1 )
                  TEMP = A(J+1,J+1,AIND)
                  CALL DLARTG( TEMP, A(J+1,J,AIND), CS1, SN1,
     $                         A(J+1,J+1,AIND) )
                  A(J+1,J,AIND) = ZERO
                  SN1 = -SN1
                  CALL DROT( J+1-IFRSTM, A(IFRSTM,J,AIND), 1,
     $                       A(IFRSTM,J+1,AIND), 1, CS1, SN1 )
               END IF
               IF ( LCMPQ ) THEN
                  QI = IWORK( MAPQ + L )
               ELSE IF ( LPARQ ) THEN
                  QI = ABS( QIND(IWORK( MAPQ + L )) )
               END IF
               IF ( QI.NE.0 ) THEN
                  CALL DROT( N, Q(1,J+1,QI), 1, Q(1,J+2,QI), 1, CS2,
     $                       SN2 )
                  CALL DROT( N, Q(1,J,QI), 1, Q(1,J+1,QI), 1, CS1, SN1 )
               END IF
  440       CONTINUE
            AIND = IWORK( MAPA + 1 )
            LM = MIN( J+3, ILASTM ) - IFRSTM + 1
            CALL DROT( LM, A(IFRSTM,J+1,AIND), 1,
     $                 A(IFRSTM,J+2,AIND), 1, CS2, SN2 )
            CALL DROT( LM, A(IFRSTM,J,AIND), 1,
     $                 A(IFRSTM,J+1,AIND), 1, CS1, SN1 )
  450    CONTINUE
C
C        To avoid IF statements, there is an extra piece of code for
C        the last step.
C
         J = ILAST - 1
         TEMP = A(J,J-1,AIND)
         CALL DLARTG( TEMP, A(J+1,J-1,AIND), CS1, SN1, A(J,J-1,AIND) )
         A(J+1,J-1,AIND) = ZERO
C
  460    CONTINUE
C
         CALL DROT( ILASTM-J+1, A(J,J,AIND), LDA1,
     $              A(J+1,J,AIND), LDA1, CS1, SN1 )
         IF ( LCMPQ ) THEN
            QI = IWORK( MAPQ + 1 )
         ELSE IF ( LPARQ ) THEN
            QI = ABS( QIND(IWORK( MAPQ + 1 )) )
         END IF
         IF ( QI.NE.0 )
     $      CALL DROT( N, Q(1,J,QI), 1, Q(1,J+1,QI), 1, CS1, SN1 )
C
C        Propagate information from the right to A_1.
C
         DO 470  L = K, 2, -1
            AIND = IWORK( MAPA + L )
            IF ( S(AIND).EQ.SINV ) THEN
               CALL DROT( J+2-IFRSTM, A(IFRSTM,J,AIND), 1,
     $                    A(IFRSTM,J+1,AIND), 1, CS1, SN1 )
               TEMP = A(J,J,AIND)
               CALL DLARTG( TEMP, A(J+1,J,AIND), CS1, SN1,
     $                      A(J,J,AIND) )
               A(J+1,J,AIND) = ZERO
               CALL DROT( ILASTM-J, A(J,J+1,AIND), LDA1,
     $                    A(J+1,J+1,AIND), LDA1, CS1, SN1 )
            ELSE
               CALL DROT( ILASTM-J+1, A(J,J,AIND), LDA1,
     $                    A(J+1,J,AIND), LDA1, CS1, SN1 )
               TEMP = A(J+1,J+1,AIND)
               CALL DLARTG( TEMP, A(J+1,J,AIND), CS1, SN1,
     $                      A(J+1,J+1,AIND) )
               A(J+1,J,AIND) = ZERO
               SN1 = -SN1
               CALL DROT( J+1-IFRSTM, A(IFRSTM,J,AIND), 1,
     $                    A(IFRSTM,J+1,AIND), 1, CS1, SN1 )
            END IF
            IF ( LCMPQ ) THEN
               QI = IWORK( MAPQ + L )
            ELSE IF ( LPARQ ) THEN
               QI = ABS( QIND(IWORK( MAPQ + L )) )
            END IF
            IF ( QI.NE.0 )
     $         CALL DROT( N, Q(1,J,QI), 1, Q(1,J+1,QI), 1, CS1, SN1 )
  470    CONTINUE
         AIND = IWORK( MAPA + 1 )
         CALL DROT( ILASTM-IFRSTM+1, A(IFRSTM,J,AIND), 1,
     $              A(IFRSTM,J+1,AIND), 1, CS1, SN1 )
C
C        End of iteration loop.
C
  480    CONTINUE
  490 CONTINUE
C
C     Drop through = non-convergence.
C
      INFO = ILAST
      GO TO 520
C
C     Successful completion of all QZ steps.
C
  500 CONTINUE
C
C     Set eigenvalues 1:ILO-1.
C
      DO 510  J = 1, ILO - 1
         CALL MA01BD( BASE, LGBAS, K, S, A(J,J,1), LDA1*LDA2, ALPHAR(J),
     $                BETA(J), SCAL(J) )
         ALPHAI(J) = ZERO
  510 CONTINUE
C
  520 CONTINUE
C
      DWORK(1) = DBLE( OPTDW )
      IWORK(1) = OPTIW
      RETURN
C *** Last line of MB03BD ***
      END


! This file is part of HolmMHD
! Copyright (C) 2019 Daniel Verscharen (d.verscharen@ucl.ac.uk)
!All rights reserved.
!
!Redistribution and use in source and binary forms, with or without
!modification, are permitted provided that the following conditions are met:
!
!1. Redistributions of source code must retain the above copyright notice, this
!   list of conditions and the following disclaimer.
!2. Redistributions in binary form must reproduce the above copyright notice,
!   this list of conditions and the following disclaimer in the documentation
!   and/or other materials provided with the distribution.
!
!THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
!ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
!WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
!DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
!ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
!(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
!LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
!ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
!(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
!SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
!
!The views and conclusions contained in the software and documentation are those
!of the authors and should not be interpreted as representing official policies,
!either expressed or implied, of the HolmMHD project.

subroutine calc_dB(U,B,dB)
use globals
implicit none

double precision :: E(3,0:Nx,0:Ny,0:Nz),U(3,-1:Nx+2,-1:Ny+2,-1:Nz+2)
double precision :: B(3,-2:Nx+2,-2:Ny+2,-2:Nz+2),dB(3,Nx,Ny,Nz)
double precision :: Ucalc(3),Bcalc(3)
integer :: i,j,k

! You get U in the middle of the cells and B on the faces of the cells.
! Calculate what is E=-UxB on the edges of the cells!
! This is the staggered grid!
!

! Define E-Field:
do i=0,Nx
do j=0,Ny
do k=0,Nz
	Ucalc(2)=0.25d0*(U(2,i,j,k)+U(2,i,j+1,k)+U(2,i,j+1,k+1)+U(2,i,j,k+1))
	Ucalc(3)=0.25d0*(U(3,i,j,k)+U(3,i,j+1,k)+U(3,i,j+1,k+1)+U(3,i,j,k+1))
	Bcalc(2)=0.5d0*(B(2,i,j,k)+B(2,i,j,k+1))
	Bcalc(3)=0.5d0*(B(3,i,j,k)+B(3,i,j+1,k))
	E(1,i,j,k)=Bcalc(2)*Ucalc(3)-Bcalc(3)*Ucalc(2)

	Ucalc(1)=0.25d0*(U(1,i,j,k)+U(1,i+1,j,k)+U(1,i+1,j,k+1)+U(1,i,j,k+1) )
	Ucalc(3)=0.25d0*(U(3,i,j,k)+U(3,i+1,j,k)+U(3,i+1,j,k+1)+U(3,i,j,k+1) )
	Bcalc(1)=0.5d0*(B(1,i,j,k)+B(1,i,j,k+1))
	Bcalc(3)=0.5d0*(B(3,i,j,k)+B(3,i+1,j,k))
	E(2,i,j,k)=Bcalc(3)*Ucalc(1)-Bcalc(1)*Ucalc(3)

	Ucalc(1)=0.25d0*(U(1,i,j,k)+U(1,i,j+1,k)+U(1,i+1,j+1,k)+U(1,i+1,j,k))
	Ucalc(2)=0.25d0*(U(2,i,j,k)+U(2,i,j+1,k)+U(2,i+1,j+1,k)+U(2,i+1,j,k))
	Bcalc(1)=0.5d0*(B(1,i,j,k)+B(1,i,j+1,k))
	Bcalc(2)=0.5d0*(B(2,i,j,k)+B(2,i+1,j,k))
	E(3,i,j,k)=Bcalc(1)*Ucalc(2)-Bcalc(2)*Ucalc(1)
enddo
enddo
enddo

do i=1,Nx
do j=1,Ny
do k=1,Nz
	dB(1,i,j,k)=(E(2,i,j,k)-E(2,i,j,k-1))/dz-(E(3,i,j,k)-E(3,i,j-1,k))/dy
	dB(2,i,j,k)=(E(3,i,j,k)-E(3,i-1,j,k))/dx-(E(1,i,j,k)-E(1,i,j,k-1))/dz
	dB(3,i,j,k)=(E(1,i,j,k)-E(1,i,j-1,k))/dy-(E(2,i,j,k)-E(2,i-1,j,k))/dx
enddo
enddo
enddo

end subroutine

subroutine B_interpol(B,Binter,i,j,k)
use globals
implicit none

double precision :: B(3,-2:Nx+2,-2:Ny+2,-2:Nz+2),Binter(3)
integer :: i,j,k

Binter(1)=0.5d0*(B(1,i,j,k)+B(1,i-1,j,k))

Binter(2)=0.5d0*(B(2,i,j,k)+B(2,i,j-1,k))

Binter(3)=0.5d0*(B(3,i,j,k)+B(3,i,j,k-1))

end subroutine


      SUBROUTINE task_circ_tor_grid()
!  task_circ_tor_grid
!    This subroutine evaluates the vector potential and
!    magnetic field on the surface of the (circular) torus described by rmajor
!    and aminor at nphi toroidal slices, ntheta times poloidally per slice.

!    By this point, the parameters (mgrid_ext, rmajor, aminor, nphi, ntheta)
!    have been input.

      USE stel_kinds

      USE stel_constants

      USE write_mgrid, only: mgrid_ext, mgrid_mode, lstell_sym,                &
     &   rmin, rmax, zmin, zmax, kp, ir, jz,                                   &
     &   coil_file, nextcur

      USE makegrid_global, only: task, rmajor, aminor, nphi, ntheta,           &
     &   extcur_mgrid, nextcur_dim

      USE biotsavart
       !  , only: coil_group
       !  subroutines: parse_coils_file
       !  access to bsc_b in module bsc.

      IMPLICIT NONE

!-----------------------------------------------
!   L o c a l   V a r i a b l e s
!-----------------------------------------------

      REAL(rprec), DIMENSION(3) :: x
      REAL(rprec), DIMENSION(3) :: btot, bdum
      REAL(rprec) :: tfrac, pfrac, brr, bpp, brout, btheta
      REAL(rprec) :: cost, sint, cosp, sinp, norm
      REAL(rprec) :: imagr, imagp, imagt, realr, realp, realt
      REAL(rprec), DIMENSION(nphi,ntheta,2) :: brf, bpf, bthetaf
      CHARACTER(LEN=80) :: ctg_file
      INTEGER :: i, j, k, nmode, mmode
      INTEGER, PARAMETER :: ctg_iou=17, fp=5
      !fp=5, do one field period and multiply phi mode numbers by 5
      !fp=1, do all field periods

!-----------------------------------------------
!  Subroutine Interface
!-----------------------------------------------

      INTERFACE
         SUBROUTINE fft_local(dat)
            USE stel_kinds
            REAL(rprec), DIMENSION(:,:,:) :: dat
         END SUBROUTINE fft_local
      END INTERFACE

!-----------------------------------------------
!  Start of Executable Code
!-----------------------------------------------

      WRITE(*,*) ' Running makegrid with the following parameters:'
      WRITE(*,*) '   task      = ', task
      WRITE(*,*) '   mgrid_ext = ', mgrid_ext
      WRITE(*,*) '   rmajor    = ', rmajor
      WRITE(*,*) '   aminor    = ', aminor
      WRITE(*,*) '   nphi      = ', nphi
      WRITE(*,*) '   ntheta    = ', ntheta
      WRITE(*,*)

      coil_file = 'coils.' // TRIM(mgrid_ext)
      WRITE (6, *) 'Input  file: ',TRIM(coil_file)

      ctg_file = 'ctg.'// TRIM(mgrid_ext)
      WRITE(*,*) 'CTG file: ', TRIM(ADJUSTL(ctg_file))
      OPEN(UNIT=ctg_iou, FILE=TRIM(ADJUSTL(ctg_file)))

      CALL parse_coils_file(coil_file)
      nextcur = SIZE(coil_group)

      x=(/0.0,0.0,0.0/)

      WRITE(ctg_iou,*) 'Fourier coefficients of magnetic field.'
      WRITE(ctg_iou,*) 'This magnetic geometry is 5 fold periodic in ',        &
     &                 'phi.  Only modes which preserve this ',                &
     &                 'periodicity are kept.'
      WRITE(ctg_iou,*) 'If the field is stellarator symmetric, the ',          &
     &                 'Fourier transform can be expressed in terms ',         &
     &                 'of only sines or only cosines.'
      WRITE(ctg_iou,*)
      WRITE(ctg_iou,*) 'Major radius: R0 = ', rmajor
      WRITE(ctg_iou,*) 'Minor radius: a = ', aminor
      WRITE(ctg_iou,*) 'nphi (number of points in phi) = ', nphi
      WRITE(ctg_iou,*) 'ntheta (number of points in theta) = ', ntheta
      WRITE(ctg_iou,*)
      WRITE(ctg_iou,*) 'Coordinate system:'
      WRITE(ctg_iou,*) 'r = SQRT((SQRT(x**2+y**2)-R0**2)**2+z**2)'
      WRITE(ctg_iou,*) 'THETA = ARCSIN(z/(SQRT(x**2+y**2)-R0)'
      WRITE(ctg_iou,*) 'PHI = ARCTAN(y/x)'
      WRITE(ctg_iou,*) 'Br = B.r_hat'
      WRITE(ctg_iou,*) 'Btheta = B.theta_hat'
      WRITE(ctg_iou,*) 'Bphi = B.phi_hat'
      WRITE(ctg_iou,*)
      WRITE(ctg_iou,*) 'The magnetic field is given by'
      WRITE(ctg_iou,*) 'Br(theta,phi) = 1/SQRT(ntheta*nphi) Sum_over_',        &
     &                 'm_and_n (BR_smn * (SIN(m*theta+n*phi))'
      WRITE(ctg_iou,*) 'Bphi(theta,phi) = 1/SQRT(ntheta*nphi) Sum_over_'       &
     &                 ,'m_and_n (BPHI_cmn * (COS(m*theta+n*phi))'
      WRITE(ctg_iou,*) 'Btheta(theta,phi) = 1/SQRT(ntheta*nphi) Sum_',         &
     &                 'over_m_and_n(BTHETA_cmn * (COS(m*theta+n*phi))'
      WRITE(ctg_iou,*) 'Where the subscripts "cmn" and"smn" is ',              &
     &                 'shorthand for the trig function involved ("c" ',       &
     &                 'or "s") and the mode number ("m", "n)'

      WRITE(ctg_iou,*)
      WRITE(ctg_iou,245) "N","M","BR_smn","BPHI_cmn","BTHETA_cmn"

! Test to make sure that nextcur <= nextcur_dim

      IF(nextcur .gt. nextcur_dim) THEN
         WRITE(*,*) 'Number of coils greater than default number of ',         &
     &              'currents.'
         STOP
      END IF
! nphi, toroidal
! ntheta, poloidal
      DO i=1, nphi
         pfrac=(i-1)*1.0/nphi
         DO j=1, ntheta
            tfrac=(j-1)*1.0/ntheta

! Define spatial position to calculate field
            cost=COS(twopi*tfrac)
            sint=SIN(twopi*tfrac)
            cosp=COS(twopi*pfrac/fp)
            sinp=SIN(twopi*pfrac/fp)
            x(1)=(rmajor+aminor*cost)*cosp
            x(2)=(rmajor+aminor*cost)*sinp
            x(3)=aminor*sint

! Calculate field
            btot=0
            DO k=1, nextcur
               CALL bsc_b(coil_group(k),x,bdum)
               btot=btot+bdum*extcur_mgrid(k)
            END DO

! Transform field to local normal coordinates
            brr=btot(1)*cosp+btot(2)*sinp
            brout=cost*brr+sint*btot(3)
            btheta=-sint*brr+cost*btot(3)
            bpp=-btot(1)*sinp+btot(2)*cosp

! Arrange fields for Fourier transform
            brf(i,j,1)=brout
            brf(i,j,2)=0
            bpf(i,j,1)=bpp
            bpf(i,j,2)=0
            bthetaf(i,j,1)=btheta
            bthetaf(i,j,2)=0

         END DO
      END DO

!Fourier transform the data in theta and phi
      CALL fft_local(brf)
      CALL fft_local(bpf)
      CALL fft_local(bthetaf)

!Arrange the data so that it runs from negative modes to positive modes and
!write to output file
      DO i=nphi/2+1, NPHI
         DO j=NTHETA/2+1, NTHETA

            norm=SQRT(nphi*one)*SQRT(ntheta*one)
            realr=brf(i,j,1)/norm
            imagr=brf(i,j,2)/norm
            realp=bpf(i,j,1)/norm
            imagp=bpf(i,j,2)/norm
            realt=bthetaf(i,j,1)/norm
            imagt=bthetaf(i,j,2)/norm

            mmode=j-NTHETA-1
            nmode=fp*(i-nphi-1)

            WRITE(ctg_iou,240) nmode, mmode, -imagr, realp, realt
         END DO
         DO j=1, NTHETA/2

            norm=SQRT(nphi*one)*SQRT(ntheta*one)
            realr=brf(i,j,1)/norm
            imagr=brf(i,j,2)/norm
            realp=bpf(i,j,1)/norm
            imagp=bpf(i,j,2)/norm
            realt=bthetaf(i,j,1)/norm
            imagt=bthetaf(i,j,2)/norm

            mmode=j-1
            nmode=fp*(i-nphi-1)

            WRITE(ctg_iou,240) nmode, mmode, -imagr, realp, realt
         END DO
      END DO

      DO i=1, NPHI/2
         DO j=NTHETA/2+1, NTHETA

            norm=SQRT(nphi*one)*SQRT(ntheta*one)
            realr=brf(i,j,1)/norm
            imagr=brf(i,j,2)/norm
            realp=bpf(i,j,1)/norm
            imagp=bpf(i,j,2)/norm
            realt=bthetaf(i,j,1)/norm
            imagt=bthetaf(i,j,2)/norm

            mmode=j-NTHETA-1
            nmode=fp*(i-1)

            WRITE(ctg_iou,240) nmode, mmode, -imagr, realp, realt
         END DO
         DO j=1, NTHETA/2

            norm=SQRT(nphi*one)*SQRT(ntheta*one)
            realr=brf(i,j,1)/norm
            imagr=brf(i,j,2)/norm
            realp=bpf(i,j,1)/norm
            imagp=bpf(i,j,2)/norm
            realt=bthetaf(i,j,1)/norm
            imagt=bthetaf(i,j,2)/norm

            mmode=j-1
            nmode=fp*(i-1)

            WRITE(ctg_iou,240) nmode, mmode, -imagr, realp, realt
         END DO
      END DO

 240  FORMAT(2I6,3ES20.12)
 245  FORMAT(2A6,3A20)
      END SUBROUTINE task_circ_tor_grid


      subroutine loop(up, u, um, f, nx, ny, Cx2, Cy2, dt2)
      integer nx, ny
      real*8 up(0:nx, 0:ny), u(0:nx, 0:ny), um(0:nx, 0:ny)
      real*8 Cx2, Cy2, dt2
Cf2py intent(in, out) up
Cf2py intent(in) u
Cf2py intent(in) um
C     special treatment of f: keep its C (row major) storage:
      real*8 f(0:ny, 0:nx)
Cf2py intent(in, c) f

      do j = 1, ny-1
         do i = 1, nx-1
            up(i,j) = - um(i,j) + 2*u(i,j) +
     &          Cx2*(u(i-1,j) - 2*u(i,j) + u(i+1,j)) +
     &          Cy2*(u(i,j-1) - 2*u(i,j) + u(i,j+1)) +
     &          dt2*f(j,i)
         end do
      end do
      return
      end


subroutine ComputeDMap(Dij, dh_mask, n_dh, lmax, sampling, exitstatus)
!------------------------------------------------------------------------------
!
!   This subroutine will compute the matrix D(lm,l'm') for the spatiospectral
!   concentration problem using the input grid DH_MASK that defines the
!   concentration region. The input grid must be sampled according to the
!   Driscoll and Healy sampling theorem and must possess values of 1 inside
!   the concentration region, and 0 elsewhere. The grid possess N_DH samples
!   in latitude, and either N_DH samples in longitude for SAMPLING=1
!   or 2*N_DH samples in longitude for SAMPLING=2. Elements of the matrix D
!   (which are ordered accoring to YilmIndex) are computed up to a maximum
!   degree LMAX.
!
!   It should be noted that the elements of D are computed approximately.
!   These are explicitly calculated using
!
!       Dlm,l'm' = 1/(4 pi) Int_Omega F Yl'm' dOmega
!
!   where
!
!       F = Ylm * DH_MASK.
!
!   This integral would be computed exactly if F had a bandwith of
!   L = N_DH/2 - 1. However, since DH_MASK has an infinite bandwidth, this
!   will not be true. However, if L is larger than LMAX, then we might expect
!   that the spherical harmonic expansions will be good approximations. This
!   needs to be verified by chosing different values of N_DH and comparing
!   the results. A simple comparison is to see how good the D(1,1) element
!   approximates the area of the concentration region.
!
!   Calling Parameters
!
!       IN
!           dh_mask     Integer grid sampled according to the Driscoll and
!                       Healy sampling theorem. A value of 1 indicates that
!                       the grid node is in the concentration domain, and a
!                       value of 0 indicates that it is outside. Dimensioned as
!                       (n_dh, n_dh) for SAMPLING = 1 or (n_dh, 2*n_dh) for
!                       SAMPLING = 2.
!           n_dh        The number of latitude samples in the Driscoll and
!                       Healy sampled grid.
!           lmax        Maximum spherical harmonic degree of elements in the
!                       matrix D.
!
!       IN, OPTIONAL
!           SAMPLING    1 (default) corresponds to equal sampling (n_dh, n_dh),
!                       whereas 2 corresponds to equal spaced grids
!                       (n_dh, 2*n_dh).
!
!       OUT
!           Dij         Elements of the matrix D, which is symmetric, and
!                       whose elements are packed into a 1D array according
!                       to YilmINDEX. Dimesions of Dij are
!                       ((lmax+1)**2, (lmax+1)**2).
!
!       OPTIONAL (OUT)
!           exitstatus  If present, instead of executing a STOP when an error
!                       is encountered, the variable exitstatus will be
!                       returned describing the error.
!                       0 = No errors;
!                       1 = Improper dimensions of input array;
!                       2 = Improper bounds for input variable;
!                       3 = Error allocating memory;
!                       4 = File IO error.
!
!   Copyright (c) 2016, SHTOOLS
!   All rights reserved.
!
!------------------------------------------------------------------------------
    use SHTOOLS, only : SHExpandDH, PlmIndex, PlmBar, SHCilmToVector, &
                        YilmIndexVector

    implicit none

    real*8, intent(out) :: Dij(:,:)
    integer, intent(in) :: dh_mask(:,:), n_dh, lmax
    integer, intent(in), optional:: sampling
    integer, intent(out), optional :: exitstatus
    integer :: nlat, nlong, lmax_dh, astat, i, j, k, l, m, max_mask, min_mask
    real*8, allocatable :: f(:,:), plm(:), clm(:,:,:), vec(:)
    real*8 :: colat, lat_int, temp(2*n_dh), lon, lon_int

    if (present(exitstatus)) exitstatus = 0

    if (size(Dij(:,1)) < (lmax+1)**2 .or. size(Dij(1,:)) < (lmax+1)**2) then
        print*, "Error --- ComputeDMap"
        print*, "Dij must be dimesioned as ((LMAX+1)**2, (LMAX+1)**2)."
        print*, "Dij is dimensioned as ", size(Dij(:,1)), size(Dij(1,:))
        if (present(exitstatus)) then
            exitstatus = 1
            return
        else
            stop
        end if

    end if

    if (mod(n_dh,2) /= 0) then
        print*, "Error --- ComputeDMap"
        print*, "Number of samples in latitude must be even for the " // &
                "Driscoll and Healy sampling theorem."
        print*, "N_DH = ", n_dh
        if (present(exitstatus)) then
            exitstatus = 2
            return
        else
            stop
        end if

    end if

    nlat = n_dh
    lat_int = acos(-1.0d0) / dble(nlat)

    if (present(sampling)) then
        if (sampling == 1) then
            nlong = nlat
            lon_int = 2.0d0 * lat_int

        else if (sampling == 2) then
            nlong = 2 * nlat
            lon_int = lat_int

        else
            print*, "Error --- ComputeDMap"
            print*, "SAMPLING must be either 1 (equally sampled) " // &
                    "or 2 (equally spaced)."
            print*, "SAMPLING = ", sampling
            if (present(exitstatus)) then
                exitstatus = 2
                return
            else
                stop
            end if

        end if

    else
        nlong = nlat
        lon_int = 2.0d0 * lat_int

    end if

    if (size(dh_mask(:,1)) < nlat .or. size(dh_mask(1,:)) < nlong) then
        print*, "Error --- ComputeDMap"
        print*, "DH_MASK must be dimensioned as ", nlat, nlong
        print*, "Dimensions of DH_MASK are ", size(dh_mask(:,1)),  &
                size(dh_mask(1,:))
        if (present(exitstatus)) then
            exitstatus = 1
            return
        else
            stop
        end if

    end if

    lmax_dh = n_dh / 2 - 1

    if (lmax_dh < lmax) then
        print*, "Error --- ComputeDMap"
        print*, "The effective bandwith of the input grid DH_MASK must be " //&
                "greater or equal than LMAX."
        print*, "LMAX = ", lmax
        print*, "Effective bandwidth of DH_MASK = ", lmax_dh
        if (present(exitstatus)) then
            exitstatus = 2
            return
        else
            stop
        end if

    endif

    allocate (f(nlat,nlong), stat = astat)

    if (astat /= 0) then
        print*, "Error --- ComputeDMap"
        print*, "Problem allocating memory for grid F(NLAT, NLONG)."
        print*, "NLAT = ", nlat
        print*, "NLONG = ", nlong
        if (present(exitstatus)) then
            exitstatus = 3
            return
        else
            stop
        end if

    end if

    allocate (plm((lmax+1)*(lmax+2)/2), stat = astat)

    if (astat /= 0) then
        print*, "Error --- ComputeDMap"
        print*, "Problem allocating memory for grid PLM((LMAX+1)*(LMAX+2)/2)."
        print*, "LMAX = ", lmax
        if (present(exitstatus)) then
            exitstatus = 3
            return
        else
            stop
        end if

    end if

    allocate (clm(2, lmax+1, lmax+1), stat = astat)

    if (astat /= 0) then
        print*, "Error --- ComputeDMap"
        print*, "Problem allocating memory for CLM(2, LMAX+1, LMAX+1)."
        if (present(exitstatus)) then
            exitstatus = 3
            return
        else
            stop
        end if

    end if

    allocate (vec((lmax+1)**2), stat = astat)

    if (astat /= 0) then
        print*, "Error --- ComputeDMap"
        print*, "Problem allocating memory for VEC((LMAX+1)**2)."
        print*, "LMAX = ", lmax
        if (present(exitstatus)) then
            exitstatus = 3
            return
        else
            stop
        end if

    end if

    min_mask = minval(dh_mask(1:nlat, 1:nlong))
    max_mask = maxval(dh_mask(1:nlat, 1:nlong))

    if (min_mask < 0 .or. max_mask > 1) then
        print*, "Error --- ComputeDMap"
        print*, "DH_MASK must consist of 0s exterior to the " // &
                "concentration region"
        print*, "and 1s inside of the concentration region."
        print*, "Minimum value of DH_MASK = ", min_mask
        print*, "Maximum value of DH_MASK = ", max_mask
        if (present(exitstatus)) then
            exitstatus = 2
            return
        else
            stop
        end if

    end if

    !--------------------------------------------------------------------------
    !
    !   Compute elements of Dij. Start at high degrees, and work towards
    !   low degrees. Dij is symmetric.
    !
    !--------------------------------------------------------------------------

    dij = 0.0d0

    do l = lmax, 0, -1
        do m = 0, l
            do j = 1, 2

                if (m == 0 .and. j == 2) cycle

                !--------------------------------------------------------------
                !
                !   Make Map of Yilm * mask
                !
                !--------------------------------------------------------------

                do k = 1, nlong
                    lon = dble(k-1) * lon_int

                    if (j == 1) then
                        temp(k) = cos(dble(m) * lon)

                    else
                        temp(k) = sin(dble(m) * lon)

                    end if

                end do

                do k = 1, nlat
                    colat = dble(k-1) * lat_int
                    if (present(exitstatus)) then
                        call plmbar(plm, l, cos(colat), exitstatus=exitstatus)
                        if (exitstatus /= 0) return
                    else
                        call plmbar(plm, l, cos(colat))
                    end if
                    f(k, 1:nlong) = plm(plmindex(l,m)) * temp(1:nlong) &
                                    * dble(dh_mask(k, 1:nlong))

                end do

                i = YilmIndexVector(j, l, m)

                if (present(exitstatus)) then
                    if (present(sampling)) then
                        call SHExpandDH(f, n_dh, clm, lmax_dh, &
                                        sampling = sampling, lmax_calc = l, &
                                        exitstatus = exitstatus)
                        if (exitstatus /= 0) return
                    else
                        call SHExpandDH(f, n_dh, clm, lmax_dh, sampling = 1, &
                                        lmax_calc = l, exitstatus = exitstatus)
                        if (exitstatus /= 0) return
                    end if
                    call SHCilmToVector(clm, vec, l, exitstatus = exitstatus)
                    if (exitstatus /= 0) return

                else
                    if (present(sampling)) then
                        call SHExpandDH(f, n_dh, clm, lmax_dh, &
                                        sampling = sampling, lmax_calc = l)
                    else
                        call SHExpandDH(f, n_dh, clm, lmax_dh, sampling = 1, &
                                        lmax_calc = l)
                    end if
                    call SHCilmToVector(clm, vec, l)

                end if

                dij(i, 1:(l+1)**2) = vec(1:(l+1)**2)

                if (l /= 0) dij(1:(l+1)**2 - 1, i) = vec(1:(l+1)**2 - 1)

            end do

        end do

    end do

    call plmbar(plm, -1, cos(colat))

    deallocate (f)
    deallocate (plm)
    deallocate (clm)
    deallocate (vec)

end subroutine ComputeDMap


!***********************************************************************
!                                                                      *
      REAL(KIND(0.0D0)) FUNCTION SKFUN (K, X)
!                                                                      *
!   Computes the function                                              *
!                                            n  nx                     *
!                             infinity   (-1)  e                       *
!                     S (x) =   Sum      -------                       *
!                               n=1          k                         *
!                                           n                          *
!                                                                      *
!   See, for instance, F. A. Parpia and A. K. Mohanty ``Relativistic   *
!   basis set calculations for atoms with Fermi nuclei'', Phys Rev A   *
!   (1992) in press.                                                   *
!                                                                      *
!   Call(s) to: SKFUN.                                                 *
!                                                                      *
!   Written by Farid A. Parpia            Last revision: 16 Oct 1994   *
!                                                                      *
!***********************************************************************
!
!...Translated by Pacific-Sierra Research 77to90  4.3E  13:31:28   1/ 3/07
!-----------------------------------------------
!   M o d u l e s
!-----------------------------------------------
      USE vast_kind_param, ONLY:  DOUBLE
      IMPLICIT NONE
!-----------------------------------------------
!   D u m m y   A r g u m e n t s
!-----------------------------------------------
      INTEGER, INTENT(IN) :: K
      REAL(DOUBLE), INTENT(IN) :: X
!-----------------------------------------------
!   L o c a l   P a r a m e t e r s
!-----------------------------------------------
      REAL(DOUBLE), PARAMETER :: QUASIZERO = 1.D-15
!-----------------------------------------------
!   L o c a l   V a r i a b l e s
!-----------------------------------------------
      REAL(DOUBLE) :: DNUMER, EN, BASE, DELTA
!-----------------------------------------------
!xhe

      DNUMER = 1.0D00
      EN = 0.0D00
      BASE = -EXP(X)
      SKFUN = 0.0D00
      DNUMER = DNUMER*BASE
      EN = EN + 1.0D00
      DELTA = DNUMER/EN**K
      SKFUN = SKFUN + DELTA
      DO WHILE(ABS(DELTA/SKFUN) > QUASIZERO)
         DNUMER = DNUMER*BASE
         EN = EN + 1.0D00
         DELTA = DNUMER/EN**K
         SKFUN = SKFUN + DELTA
!xhb
!xh      IF (ABS (DELTA/SKFUN) .GT. 1.0D-15 ) GOTO 1
      END DO
!xhe
!
      RETURN
      END FUNCTION SKFUN


*DECK C0LGMC
      COMPLEX FUNCTION C0LGMC (Z)
C***BEGIN PROLOGUE  C0LGMC
C***PURPOSE  Evaluate (Z+0.5)*LOG((Z+1.)/Z) - 1.0 with relative
C            accuracy.
C***LIBRARY   SLATEC (FNLIB)
C***CATEGORY  C7A
C***TYPE      COMPLEX (C0LGMC-C)
C***KEYWORDS  FNLIB, GAMMA FUNCTION, SPECIAL FUNCTIONS
C***AUTHOR  Fullerton, W., (LANL)
C***DESCRIPTION
C
C Evaluate  (Z+0.5)*LOG((Z+1.0)/Z) - 1.0  with relative error accuracy
C Let Q = 1.0/Z so that
C     (Z+0.5)*LOG(1+1/Z) - 1 = (Z+0.5)*(LOG(1+Q) - Q + Q*Q/2) - Q*Q/4
C        = (Z+0.5)*Q**3*C9LN2R(Q) - Q**2/4,
C where  C9LN2R  is (LOG(1+Q) - Q + 0.5*Q**2) / Q**3.
C
C***REFERENCES  (NONE)
C***ROUTINES CALLED  C9LN2R, R1MACH
C***REVISION HISTORY  (YYMMDD)
C   780401  DATE WRITTEN
C   890531  Changed all specific intrinsics to generic.  (WRB)
C   890531  REVISION DATE from Version 3.2
C   891214  Prologue converted to Version 4.0 format.  (BAB)
C***END PROLOGUE  C0LGMC
      COMPLEX Z, Q, C9LN2R
      SAVE RBIG
      DATA RBIG / 0.0 /
C***FIRST EXECUTABLE STATEMENT  C0LGMC
      IF (RBIG.EQ.0.0) RBIG = 1.0/R1MACH(3)
C
      CABSZ = ABS(Z)
      IF (CABSZ.GT.RBIG) C0LGMC = -(Z+0.5)*LOG(Z) - Z
      IF (CABSZ.GT.RBIG) RETURN
C
      Q = 1.0/Z
      IF (CABSZ.LE.1.23) C0LGMC = (Z+0.5)*LOG(1.0+Q) - 1.0
      IF (CABSZ.GT.1.23) C0LGMC = ((1.+.5*Q)*C9LN2R(Q) - .25) * Q**2
C
      RETURN
      END


program trajectories

  ! Original version by Alan Nathan
  ! Modifcations by Chris Long

  parameter(noptmax=7)
  !parameter(pi=3.1415926,twopi=2.*pi,rad=pi/180,ftm=12./39.37,
  !  mtf=1/ftm,rpm=60./twopi)
  parameter(pi=3.1415926)
  parameter(twopi=2.*pi)
  parameter(rad=pi/180,ftm=12./39.37,mtf=1/ftm,rpm=60./twopi)
  parameter(ndmax=200)

  real, dimension(1:ndmax)::rhodata,vdata,thetadata,vwind
  common/data/ rhodata,vdata,thetadata,vwind
  common/flag/iflag
  parameter(const0=5.283e-3,rho0=1.175)
  common/spin/ctht,stht,cphi,sphi

  ! local version of best fit trajectory parameters
  common/fitting/cd0,cddot,cdspin,w0,theta0,tau0

  ! local version of input parameters
  ! in mph,det,kg/m^3,mph,mph
  common/input/v0,theta,rho,const,vwx,vwy

  ! in ft/s^2,ft
  common/constants/g,radius

  parameter(nterms=8,npts=1000)
  common/initial/xstart(nterms)
  common/path/t(npts),x(nterms,npts)

  ! ft/s^2
  g=32.179
  circ=9.125
  ! radius of ball in ft
  radius=circ/twopi/12

  ! no wind
  vwx=0.
  vwy=0.

  ! index i:
  !   1,2,3 ==> xi,yi,zi
  !   4,5,6 ==> vx,vy,vz
  !   7,8 ==> r*wb,r*ws

  ! wb=backspin rate
  ! ws=sidespin rate

  ! note on the right-handed coordinate:
  !   z is vertical with positive upward
  !   y is catcher-pitcher line
  !   x points to catcher's right

  ! NOTE:  This current version is 2D, so that x=vx=ws=0
  ! It is easy to reinstate these to make it full 3D

  ! read in parameters for Cd,wb

  ! input paramters
  open(unit=20,file='parameters.csv',status='old')
  read(20,*)
  read(20,*)cd0,cddot,cdspin,w0,theta0,tau0
  close(20)

  ! output file
  !open(unit=21,file='TrajectoryCalculatorOutput.csv',status='unknown')
  ! Default status should be unknown
  ! output file
  open(unit=21,file='TrajectoryCalculatorOutput.csv')

  open(unit=20,file='trajectory_input.csv',status='old')
  ! input data
  ! skip header
  read(20,*)
  do i=1,ndmax
     read(20,*,end=200)rhodata(i),vdata(i),thetadata(i),vwind(i)
     !print*,rhodata(i),vdata(i),thetadata(i),vwind(i)
  end do
200		ndata=i-1
		tmax=10.
		write(21,251)		!write header
		do i=1,ndata
			rho=rhodata(i)
			const=const0*(rho/rho0)
			v0=vdata(i)
			theta=thetadata(i)
			vwy=vwind(i)*1.467
			vwx=0.
			xstart(1)=0.
			xstart(2)=2.
			xstart(3)=3.
			xstart(4)=0.
			ctht=cos(theta*rad)
			stht=sin(theta*rad)
			xstart(5)=(v0*1.467)*ctht
			xstart(6)=(v0*1.467)*stht
			cphi=1.
			sphi=0.
			spin=(w0/rpm)*(v0/100.)*(theta-theta0)/(27.5-theta0)
			xstart(7)=radius*spin
			xstart(8)=0.
c
c			spin rate=w0 at v0=100. and theta=27.5 and 0 at theta=aparm(5)
c
c			integrate to get trajectory, then distance
c
			call rkdumb(xstart,nterms,0.,tmax,npts)
			call dist(range,time)
			sfact=xstart(7)/(1.467*v0)
			cdi=cd0*(1.+cddot*(100.-v0))*(1.+cdspin*sfact**2)
			write(21,250)rho,v0,theta,spin*rpm,sfact,cdi,vwind(i),range,time
			write(6,250)rho,v0,theta,spin*rpm,sfact,cdi,vwind(i),range,time
		enddo
		go to 100
	elseif(nopt.eq.6) then
		print*,' enter fixed rho,v0,theta,vwnd'
		read*, rho,v0,theta,vwnd
		print*,' enter wmin,wmax,nsteps'
		read*,wmin,wmax,nsteps
		dw=(wmax-wmin)/float(nsteps)
		vwy=vwnd*1.467
		vwx=0.
		tmax=10.
		do i=1,nsteps+1
			const=const0*(rho/rho0)
			spin=(wmin+float(i-1)*dw)/rpm
			xstart(1)=0.
			xstart(2)=2.
			xstart(3)=3.
			xstart(4)=0.
			ctht=cos(theta*rad)
			stht=sin(theta*rad)
			xstart(5)=(v0*1.467)*ctht
			xstart(6)=(v0*1.467)*stht
			cphi=1.
			sphi=1.
			xstart(7)=radius*spin
			xstart(8)=0.
			ctht=cos(theta)
			stht=sin(theta)
			cphi=1.
			sphi=0.

			call rkdumb(xstart,nterms,0.,tmax,npts)
			call dist(range,time)
			sfact=xstart(7)/(1.467*v0)
			cdi=cd0*(1.+cddot*(100.-v0))*(1.+cdspin*sfact**2)
			write(21,250)rho,v0,theta,spin*rpm,sfact,cdi,vwnd,range,time
			write(6,250)rho,v0,theta,spin*rpm,sfact,cdi,vwnd,range,time
		enddo
		go to 100
	elseif(nopt.eq.7) then
		print*,' enter fixed rho,vwind (kg/m^3,mph)'
		read*,rho,vwnd
		const=const0*(rho/rho0)
		vwy=vwnd*1.467
		vwx=0					!2D
		print*,' enter vmin,vmax,dv (mph)'
		read*,vmin,vmax,dv
		print*,' enter thetamin,thetamax,dtheta (deg)'
		read*,thetamin,thetamax,dtheta
		v0=vmin
		do while(v0.le.vmax)
			theta=thetamin
			do while(theta.le.thetamax)
				xstart(1)=0.
				xstart(2)=2.
				xstart(3)=3.
				xstart(4)=0.
				ctht=cos(theta*rad)
				stht=sin(theta*rad)
				xstart(5)=(v0*1.467)*ctht
				xstart(6)=(v0*1.467)*stht
				cphi=1.
				sphi=0.
				spin=(w0/rpm)*(v0/100.)*(theta-theta0)/(27.5-theta0)
				xstart(7)=radius*spin
				xstart(8)=0.
				tmax=10.
				call rkdumb(xstart,nterms,0.,tmax,npts)
				call dist(range,time)
				write(21,252)v0,theta,range,time
				write(6,252)v0,theta,range,time
				theta=theta+dtheta
			enddo
			v0=v0+dv
		enddo
	endif
	go to 100
c
251	format(' rho, v0, theta, spin, sfact, cdi, vwind, distance, tof')
250	format(f8.3,',',2(f8.1,','),f8.0,',',2(f8.3,','),10(f8.1,','))
252	format(4(f8.3,','))
	close(21)
	stop
	end
c
*
	subroutine derivs(t,x,dxdt)
*
*	Given the values of t and x, returns derivatives dxdt=dv/dt
*
c
c	local version of fitting parameters
c
	common/fitting/cd0,cddot,cdspin,w0,theta0,tau0
c
c	local version of input parameters
c
	common/input/v0,theta,rho,const,vwx,vwy			!in mph,det,kg/m^3,mph,mph
c
	common/constants/g,radius			!in ft/s^2,ft
	common/spin/ctht,stht,cphi,sphi

c

	dimension x(8),dxdt(8)
	real drag,lift
	parameter(pi=3.1415926)
	parameter(twopi=2.*pi)
	parameter(rad=pi/180,ftm=12./39.37,rpm=60./twopi)
*
*		1,2,4 ==> x,y,z
*		4,5,6 ==> vx,vy,vz
*		7,8 ==> r*wb,r*ws
*
*	NOTE:  this is 2D problem, so x(1),dxdt(1) are both fixed at 0
*
c
c	vwx,wwy are x,y components of wind speed
c
c	components of radius*spin
c
	rwb=x(7)
	rws=x(8)
	romega=sqrt(rwb**2+rws**2)
	vt=sqrt((x(4)-vwx)**2+(x(5)-vwy)**2+x(6)**2)	!speed of ball
	sfact=romega/vt				!spin factor
c
c	note that spin is pure backspin (i.e, rws=0)
c
	cd=cd0*(1.+cddot*(100.-v0))*(1.+cdspin*sfact**2)
	cl=sfact/(2.32*sfact+0.4)	!Cross prescription
c	print*,cd,cl,sfact,g,x(7),vt
c	pause
	drag=const*cd*vt		!actually drag/(m*vt)
	lift=const*cl*vt		!actually lift/(m*vt)
c
c	components of spin
c	NOTE:  For 2D, ws=0; cphi=1; sphi=0; this means wy=yz=0 and wx=wb
c
	wx1=rwb
	wy1=-rws*stht
	wz=rws*ctht
	wx=wx1*cphi+wy1*sphi
	wy=-wx1*sphi+wy1*cphi
c
c	the preceding five statement assure the the spin vector is normal to the initial velocity vector
c

	liftx=lift*(wy*x(6)-wz*(x(5)-vwy))/romega		!=0 for 2D
	lifty=lift*(wz*(x(4)-vwx)-wx*x(6))/romega
	liftz=lift*(wx*(x(5)-vwy)-wy*(x(4)-vwx))/romega
	dxdt(1)=x(4)							!=0 for 2D
	dxdt(2)=x(5)
	dxdt(3)=x(6)
	dxdt(4)=-drag*(x(4)-vwx)+liftx				!=0 for 2D
	dxdt(5)=-drag*(x(5)-vwy)+lifty
	dxdt(6)=-drag*x(6)+liftz-g
	dxdt(7)=0.								!no spin-down
	dxdt(8)=0.								!no spin-down

	return
	end
*
*
	subroutine RK4(Y,DYDX,N,X,H,YOUT)
*
*	Given values for N variables y and their derivatives DYDX known
*	at X, use the fourth-order Runge-Kutta method to advance the
*	solution over an interval H and return the incremented variables
*	as YOUT, which need not be a distinct array from Y.  The user
*	supplies the subroutine DERIVS(X,Y,DYDX) which returns the
*	derivatives DYDX at X
*
*	Routine taken from Numerical Recipes, 15.1, p. 553
*
	parameter (nmax=10)	!set to the maximum number of functions
	dimension Y(N),DYDX(N),YOUT(N),YT(NMAX),DYT(NMAX),DYM(NMAX)
	hh=h*0.5
	h6=h/6.
	xh=x+hh
	do i=1,n				!first step
		yt(i)=y(i)+hh*dydx(i)
	enddo
	call derivs(xh,yt,dyt)			!second step
	do i=1,n
		yt(i)=y(i)+hh*dyt(i)
	enddo
	call derivs(xh,yt,dym)			!third step
	do i=1,n
		yt(i)=y(i)+h*dym(i)
		dym(i)=dyt(i)+dym(i)
	enddo
	call derivs(x+h,yt,dyt)			!fourth step
	do i=1,n
		yout(i)=y(i)+h6*(dydx(i)+dyt(i)+2.*dym(i))
	enddo
	return
	end
*
*
	subroutine rkdumb(vstart,nvar,x1,x2,nstep)
*
*	Starting from initial values VSTART for NVAR functions, known at
*	X1, use fourth-order Runge-Kutta to advance npts equal increments
*	to X2.  The user supplied subroutine DERIVS(X,V,DVDX) evaluates
*	derivatives.  Results are stored in the common block PATH.  Be sure
*	to dimension the common block appropriately
*
*
*	Routine taken from Numerical Recipes, 15.1, p. 554
	common/coeff/cdprime,cm,cl,rey,sfact,vt
*
	parameter(nmax=10)		!set to the maximum number of functions
	parameter(nterms=8)
	parameter(npts=1000)
	common/path/xx(npts),y(nterms,npts) !nterms functions; npts values
	dimension vstart(nvar),v(nmax),dv(nmax)
	do i=1,nvar			!load starting values
		v(i)=vstart(i)
		y(i,1)=v(i)
	enddo
	xx(1)=x1
	x=x1
	h=(x2-x1)/nstep
c	print*,xx(1),y(1,1),y(2,1)
	do k=1,nstep-1			!take nsetp steps
		call derivs(x,v,dv)
		call rk4(v,dv,nvar,x,h,v)
		if(x+h.eq.x)pause 'Stepsize not significant in RKDUMB'
		x=x+h
		xx(k+1)=x		!store intermediate steps
		do i=1,nvar
			y(i,k+1)=v(i)
		enddo
c
c	diagnostic print statements
c
c		write(6,102)xx(k+1),(y(ii,k+1),ii=1,2),(y(ii,k+1),ii=4,5),
c    1		y(7,k+1),cd,cm
c		write(6,102)cd,cm,cl,sfact,vt,rey
102	format(10(f10.3,1x))

	enddo
	return
	end
*
	subroutine dist(range,time)
	parameter(nterms=8,pi=3.1415926,twopi=2.*pi,rad=pi/180.)
	parameter(npts=1000)
	common/path/t(npts),x(nterms,npts)
	do i=1,npts
		if(x(3,i).le.0..and.i.gt.1) go to 250
	enddo
250   frac=x(3,i-1)/(x(3,i-1)-x(3,i))!linear interpolation to get range
	d1=sqrt(x(2,i-1)**2+x(1,i-1)**2)
	d2=sqrt(x(2,i)**2+x(1,i)**2)
	range=d1+frac*(d2-d1)
	time=t(i)
	return
	end


!-----------------------------------------------------------------------
      subroutine ppiclf_user_SetYdot
!
      implicit none
!
      include "PPICLF"
!
! Internal:
!
      real*8 fqsy,fby
      integer*4 i
!
! evaluate ydot
      do i=1,ppiclf_npart
         ! Stokes drag
         fqsy = -ppiclf_y(PPICLF_JVY,i)/ppiclf_rprop(PPICLF_R_JTAUP,i)

         ! Gravity
         fby  = -9.8d0

         ! set ydot for all PPICLF_LRS number of equations
         ppiclf_ydot(PPICLF_JX ,i) = 0.0d0
         ppiclf_ydot(PPICLF_JY ,i) = ppiclf_y(PPICLF_JVY,i)
         ppiclf_ydot(PPICLF_JZ ,i) = 0.0d0
         ppiclf_ydot(PPICLF_JVY,i) = fqsy+fby
      enddo
! evaluate ydot

      return
      end
!-----------------------------------------------------------------------
      subroutine ppiclf_user_MapProjPart(map,y,ydot,ydotc,rprop)
!
      implicit none
!
! Input:
!
      real*8 y    (PPICLF_LRS)
      real*8 ydot (PPICLF_LRS)
      real*8 ydotc(PPICLF_LRS)
      real*8 rprop(PPICLF_LRP)
!
! Output:
!
      real*8 map  (PPICLF_LRP_PRO)
!

      map(PPICLF_P_JTEST) = rprop(PPICLF_R_JTAUP)

      return
      end
!-----------------------------------------------------------------------
      subroutine ppiclf_user_EvalNearestNeighbor
     >                                        (i,j,yi,rpropi,yj,rpropj)
!
      implicit none
!
      include "PPICLF"
!
! Input:
!
      integer*4 i
      integer*4 j
      real*8 yi    (PPICLF_LRS)
      real*8 rpropi(PPICLF_LRP)
      real*8 yj    (PPICLF_LRS)
      real*8 rpropj(PPICLF_LRP)
!

      return
      end
!-----------------------------------------------------------------------