      SUBROUTINE TB04BD( JOBD, ORDER, EQUIL, N, M, P, MD, A, LDA, B,
     $                   LDB, C, LDC, D, LDD, IGN, LDIGN, IGD, LDIGD,
     $                   GN, GD, TOL, IWORK, DWORK, LDWORK, INFO )
C
C     SLICOT RELEASE 5.7.
C
C     Copyright (c) 2002-2020 NICONET e.V.
C
C     PURPOSE
C
C     To compute the transfer function matrix G of a state-space
C     representation (A,B,C,D) of a linear time-invariant multivariable
C     system, using the pole-zeros method. Each element of the transfer
C     function matrix is returned in a cancelled, minimal form, with
C     numerator and denominator polynomials stored either in increasing
C     or decreasing order of the powers of the indeterminate.
C
C     ARGUMENTS
C
C     Mode Parameters
C
C     JOBD    CHARACTER*1
C             Specifies whether or not a non-zero matrix D appears in
C             the given state-space model:
C             = 'D':  D is present;
C             = 'Z':  D is assumed to be a zero matrix.
C
C     ORDER   CHARACTER*1
C             Specifies the order in which the polynomial coefficients
C             are stored, as follows:
C             = 'I':  Increasing order of powers of the indeterminate;
C             = 'D':  Decreasing order of powers of the indeterminate.
C
C     EQUIL   CHARACTER*1
C             Specifies whether the user wishes to preliminarily
C             equilibrate the triplet (A,B,C) as follows:
C             = 'S':  perform equilibration (scaling);
C             = 'N':  do not perform equilibration.
C
C     Input/Output Parameters
C
C     N       (input) INTEGER
C             The order of the system (A,B,C,D).  N >= 0.
C
C     M       (input) INTEGER
C             The number of the system inputs.  M >= 0.
C
C     P       (input) INTEGER
C             The number of the system outputs.  P >= 0.
C
C     MD      (input) INTEGER
C             The maximum degree of the polynomials in G, plus 1. An
C             upper bound for MD is N+1.  MD >= 1.
C
C     A       (input/output) DOUBLE PRECISION array, dimension (LDA,N)
C             On entry, the leading N-by-N part of this array must
C             contain the original state dynamics matrix A.
C             On exit, if EQUIL = 'S', the leading N-by-N part of this
C             array contains the balanced matrix inv(S)*A*S, as returned
C             by SLICOT Library routine TB01ID.
C             If EQUIL = 'N', this array is unchanged on exit.
C
C     LDA     INTEGER
C             The leading dimension of array A.  LDA >= MAX(1,N).
C
C     B       (input/output) DOUBLE PRECISION array, dimension (LDB,M)
C             On entry, the leading N-by-M part of this array must
C             contain the input matrix B.
C             On exit, the contents of B are destroyed: all elements but
C             those in the first row are set to zero.
C
C     LDB     INTEGER
C             The leading dimension of array B.  LDB >= MAX(1,N).
C
C     C       (input/output) DOUBLE PRECISION array, dimension (LDC,N)
C             On entry, the leading P-by-N part of this array must
C             contain the output matrix C.
C             On exit, if EQUIL = 'S', the leading P-by-N part of this
C             array contains the balanced matrix C*S, as returned by
C             SLICOT Library routine TB01ID.
C             If EQUIL = 'N', this array is unchanged on exit.
C
C     LDC     INTEGER
C             The leading dimension of array C.  LDC >= MAX(1,P).
C
C     D       (input) DOUBLE PRECISION array, dimension (LDD,M)
C             If JOBD = 'D', the leading P-by-M part of this array must
C             contain the matrix D.
C             If JOBD = 'Z', the array D is not referenced.
C
C     LDD     INTEGER
C             The leading dimension of array D.
C             LDD >= MAX(1,P), if JOBD = 'D';
C             LDD >= 1,        if JOBD = 'Z'.
C
C     IGN     (output) INTEGER array, dimension (LDIGN,M)
C             The leading P-by-M part of this array contains the degrees
C             of the numerator polynomials in the transfer function
C             matrix G. Specifically, the (i,j) element of IGN contains
C             the degree of the numerator polynomial of the transfer
C             function G(i,j) from the j-th input to the i-th output.
C
C     LDIGN   INTEGER
C             The leading dimension of array IGN.  LDIGN >= max(1,P).
C
C     IGD     (output) INTEGER array, dimension (LDIGD,M)
C             The leading P-by-M part of this array contains the degrees
C             of the denominator polynomials in the transfer function
C             matrix G. Specifically, the (i,j) element of IGD contains
C             the degree of the denominator polynomial of the transfer
C             function G(i,j).
C
C     LDIGD   INTEGER
C             The leading dimension of array IGD.  LDIGD >= max(1,P).
C
C     GN      (output) DOUBLE PRECISION array, dimension (P*M*MD)
C             This array contains the coefficients of the numerator
C             polynomials, Num(i,j), of the transfer function matrix G.
C             The polynomials are stored in a column-wise order, i.e.,
C             Num(1,1), Num(2,1), ..., Num(P,1), Num(1,2), Num(2,2),
C             ..., Num(P,2), ..., Num(1,M), Num(2,M), ..., Num(P,M);
C             MD memory locations are reserved for each polynomial,
C             hence, the (i,j) polynomial is stored starting from the
C             location ((j-1)*P+i-1)*MD+1. The coefficients appear in
C             increasing or decreasing order of the powers of the
C             indeterminate, according to ORDER.
C
C     GD      (output) DOUBLE PRECISION array, dimension (P*M*MD)
C             This array contains the coefficients of the denominator
C             polynomials, Den(i,j), of the transfer function matrix G.
C             The polynomials are stored in the same way as the
C             numerator polynomials.
C
C     Tolerances
C
C     TOL     DOUBLE PRECISION
C             The tolerance to be used in determining the
C             controllability of a single-input system (A,b) or (A',c'),
C             where b and c' are columns in B and C' (C transposed). If
C             the user sets TOL > 0, then the given value of TOL is used
C             as an absolute tolerance; elements with absolute value
C             less than TOL are considered neglijible. If the user sets
C             TOL <= 0, then an implicitly computed, default tolerance,
C             defined by TOLDEF = N*EPS*MAX( NORM(A), NORM(bc) ) is used
C             instead, where EPS is the machine precision (see LAPACK
C             Library routine DLAMCH), and bc denotes the currently used
C             column in B or C' (see METHOD).
C
C     Workspace
C
C     IWORK   INTEGER array, dimension (N)
C
C     DWORK   DOUBLE PRECISION array, dimension (LDWORK)
C             On exit, if INFO = 0, DWORK(1) returns the optimal value
C             of LDWORK.
C
C     LDWORK  INTEGER
C             The length of the array DWORK.
C             LDWORK >= MAX(1, N*(N+P) +
C                              MAX( N + MAX( N,P ), N*(2*N+5)))
C             If N >= P, N >= 1, the formula above can be written as
C             LDWORK >= N*(3*N + P + 5).
C             For optimum performance LDWORK should be larger.
C
C     Error Indicator
C
C     INFO    INTEGER
C             = 0:  successful exit;
C             < 0:  if INFO = -i, the i-th argument had an illegal
C                   value;
C             = 1:  the QR algorithm failed to converge when trying to
C                   compute the zeros of a transfer function;
C             = 2:  the QR algorithm failed to converge when trying to
C                   compute the poles of a transfer function.
C                   The errors INFO = 1 or 2 are unlikely to appear.
C
C     METHOD
C
C     The routine implements the pole-zero method proposed in [1].
C     This method is based on an algorithm for computing the transfer
C     function of a single-input single-output (SISO) system.
C     Let (A,b,c,d) be a SISO system. Its transfer function is computed
C     as follows:
C
C     1) Find a controllable realization (Ac,bc,cc) of (A,b,c).
C     2) Find an observable realization (Ao,bo,co) of (Ac,bc,cc).
C     3) Compute the r eigenvalues of Ao (the poles of (Ao,bo,co)).
C     4) Compute the zeros of (Ao,bo,co,d).
C     5) Compute the gain of (Ao,bo,co,d).
C
C     This algorithm can be implemented using only orthogonal
C     transformations [1]. However, for better efficiency, the
C     implementation in TB04BD uses one elementary transformation
C     in Step 4 and r elementary transformations in Step 5 (to reduce
C     an upper Hessenberg matrix to upper triangular form). These
C     special elementary transformations are numerically stable
C     in practice.
C
C     In the multi-input multi-output (MIMO) case, the algorithm
C     computes each element (i,j) of the transfer function matrix G,
C     for i = 1 : P, and for j = 1 : M. For efficiency reasons, Step 1
C     is performed once for each value of j (each column of B). The
C     matrices Ac and Ao result in Hessenberg form.
C
C     REFERENCES
C
C     [1] Varga, A. and Sima, V.
C         Numerically Stable Algorithm for Transfer Function Matrix
C         Evaluation.
C         Int. J. Control, vol. 33, nr. 6, pp. 1123-1133, 1981.
C
C     NUMERICAL ASPECTS
C
C     The algorithm is numerically stable in practice and requires about
C     20*N**3 floating point operations at most, but usually much less.
C
C     FURTHER COMMENTS
C
C     For maximum efficiency of index calculations, GN and GD are
C     implemented as one-dimensional arrays.
C
C     CONTRIBUTORS
C
C     V. Sima, Research Institute for Informatics, Bucharest, May 2002.
C     Partly based on the BIMASC Library routine TSMT1 by A. Varga.
C
C     REVISIONS
C
C     -
C
C     KEYWORDS
C
C     Eigenvalue, state-space representation, transfer function, zeros.
C
C     ******************************************************************
C
C     .. Parameters ..
      DOUBLE PRECISION   ZERO, ONE, C100
      PARAMETER          ( ZERO = 0.0D0, ONE = 1.0D0, C100 = 100.0D0 )
C     .. Scalar Arguments ..
      CHARACTER          EQUIL, JOBD, ORDER
      DOUBLE PRECISION   TOL
      INTEGER            INFO, LDA, LDB, LDC, LDD, LDIGD, LDIGN, LDWORK,
     $                   M, MD, N, P
C     .. Array Arguments ..
      DOUBLE PRECISION   A(LDA,*), B(LDB,*), C(LDC,*), D(LDD,*),
     $                   DWORK(*), GD(*), GN(*)
      INTEGER            IGD(LDIGD,*), IGN(LDIGN,*), IWORK(*)
C     .. Local Scalars ..
      DOUBLE PRECISION   ANORM, DIJ, EPSN, MAXRED, TOLDEF, X
      INTEGER            I, IA, IAC, IAS, IB, IC, ICC, IERR, IIP, IM,
     $                   IP, IPM1, IRP, ITAU, ITAU1, IZ, J, JJ, JWORK,
     $                   JWORK1, K, L, NCONT, WRKOPT
      LOGICAL            ASCEND, DIJNZ, FNDEIG, WITHD
C     .. Local Arrays ..
      DOUBLE PRECISION   Z(1)
C     .. External Functions ..
      LOGICAL            LSAME
      DOUBLE PRECISION   DLAMCH, DLANGE
      EXTERNAL           DLAMCH, DLANGE, LSAME
C     .. External Subroutines ..
      EXTERNAL           DAXPY, DCOPY, DHSEQR, DLACPY, MA02AD, MC01PD,
     $                   MC01PY, TB01ID, TB01ZD, TB04BX, XERBLA
C     .. Intrinsic Functions ..
      INTRINSIC          ABS, DBLE, INT, MAX, MIN
C     ..
C     .. Executable Statements ..
C
C     Test the input scalar parameters.
C
      INFO   = 0
      WITHD  = LSAME( JOBD,  'D' )
      ASCEND = LSAME( ORDER, 'I' )
      IF( .NOT.WITHD .AND. .NOT.LSAME( JOBD, 'Z' ) ) THEN
         INFO = -1
      ELSE IF( .NOT.ASCEND .AND. .NOT.LSAME( ORDER, 'D' ) ) THEN
         INFO = -2
      ELSE IF( .NOT. ( LSAME( EQUIL, 'S' ) .OR.
     $                 LSAME( EQUIL, 'N' ) ) ) THEN
         INFO = -3
      ELSE IF( N.LT.0 ) THEN
         INFO = -4
      ELSE IF( M.LT.0 ) THEN
         INFO = -5
      ELSE IF( P.LT.0 ) THEN
         INFO = -6
      ELSE IF( MD.LT.1 ) THEN
         INFO = -7
      ELSE IF( LDA.LT.MAX( 1, N ) ) THEN
         INFO = -9
      ELSE IF( LDB.LT.MAX( 1, N ) ) THEN
         INFO = -11
      ELSE IF( LDC.LT.MAX( 1, P ) ) THEN
         INFO = -13
      ELSE IF( LDD.LT.1 .OR. ( WITHD .AND. LDD.LT.P ) ) THEN
         INFO = -15
      ELSE IF( LDIGN.LT.MAX( 1, P ) ) THEN
         INFO = -17
      ELSE IF( LDIGD.LT.MAX( 1, P ) ) THEN
         INFO = -19
      ELSE IF( LDWORK.LT.MAX( 1, N*( N + P ) +
     $                           MAX( N + MAX( N, P ), N*( 2*N + 5 ) ) )
     $       ) THEN
         INFO = -25
      END IF
C
      IF ( INFO.NE.0 ) THEN
C
C        Error return.
C
         CALL XERBLA( 'TB04BD', -INFO )
         RETURN
      END IF
C
C     Initialize GN and GD to zero.
C
      Z(1) = ZERO
      CALL DCOPY( P*M*MD, Z, 0, GN, 1 )
      CALL DCOPY( P*M*MD, Z, 0, GD, 1 )
C
C     Quick return if possible.
C
      IF( MIN( N, P, M ).EQ.0 ) THEN
         IF( MIN( P, M ).GT.0 ) THEN
            K = 1
C
            DO 20 J = 1, M
C
               DO 10 I = 1, P
                  IGN(I,J) = 0
                  IGD(I,J) = 0
                  IF ( WITHD )
     $               GN(K) = D(I,J)
                  GD(K) = ONE
                  K = K + MD
   10          CONTINUE
C
   20       CONTINUE
C
         END IF
         DWORK(1) = ONE
         RETURN
      END IF
C
C     Prepare the computation of the default tolerance.
C
      TOLDEF = TOL
      IF( TOLDEF.LE.ZERO ) THEN
         EPSN  = DBLE( N )*DLAMCH( 'Epsilon' )
         ANORM = DLANGE( 'Frobenius', N, N, A, LDA, DWORK )
      END IF
C
C     Initializations.
C
      IA    = 1
      IC    = IA + N*N
      ITAU  = IC + P*N
      JWORK = ITAU + N
      IAC   = ITAU
C
      K   = 1
      DIJ = ZERO
C
C     (Note: Comments in the code beginning "Workspace:" describe the
C     minimal amount of real workspace needed at that point in the
C     code, as well as the preferred amount for good performance.)
C
      IF( LSAME( EQUIL, 'S' ) ) THEN
C
C        Scale simultaneously the matrices A, B and C:
C        A <- inv(S)*A*S,  B <- inv(S)*B and C <- C*S, where S is a
C        diagonal scaling matrix.
C        Workspace: need   N.
C
         MAXRED = C100
         CALL TB01ID( 'All', N, M, P, MAXRED, A, LDA, B, LDB, C, LDC,
     $                DWORK, IERR )
      END IF
C
C     Compute the transfer function matrix of the system (A,B,C,D).
C
      DO 80 J = 1, M
C
C        Save A and C.
C        Workspace: need   W1 = N*(N+P).
C
         CALL DLACPY( 'Full', N, N, A, LDA, DWORK(IA), N )
         CALL DLACPY( 'Full', P, N, C, LDC, DWORK(IC), P )
C
C        Remove the uncontrollable part of the system (A,B(J),C).
C        Workspace: need   W1+N+MAX(N,P);
C                   prefer larger.
C
         CALL TB01ZD( 'No Z', N, P, DWORK(IA), N, B(1,J), DWORK(IC), P,
     $                NCONT, Z, 1, DWORK(ITAU), TOL, DWORK(JWORK),
     $                LDWORK-JWORK+1, IERR )
         IF ( J.EQ.1 )
     $      WRKOPT = INT( DWORK(JWORK) ) + JWORK - 1
C
         IB     = IAC   + NCONT*NCONT
         ICC    = IB    + NCONT
         ITAU1  = ICC   + NCONT
         IRP    = ITAU1
         IIP    = IRP   + NCONT
         IAS    = IIP   + NCONT
         JWORK1 = IAS   + NCONT*NCONT
C
         DO 70 I = 1, P
            IF ( WITHD )
     $         DIJ = D(I,J)
            IF ( NCONT.GT.0 ) THEN
C
C              Form the matrices of the state-space representation of
C              the dual system for the controllable part.
C              Workspace: need   W2 = W1+N*(N+2).
C
               CALL MA02AD( 'Full', NCONT, NCONT, DWORK(IA), N,
     $                      DWORK(IAC), NCONT )
               CALL DCOPY( NCONT, B(1,J), 1, DWORK(IB), 1 )
               CALL DCOPY( NCONT, DWORK(IC+I-1), P, DWORK(ICC), 1 )
C
C              Remove the unobservable part of the system (A,B(J),C(I)).
C              Workspace: need   W2+2*N;
C                         prefer larger.
C
               CALL TB01ZD( 'No Z', NCONT, 1, DWORK(IAC), NCONT,
     $                      DWORK(ICC), DWORK(IB), 1, IP, Z, 1,
     $                      DWORK(ITAU1), TOL, DWORK(IIP), LDWORK-IIP+1,
     $                      IERR )
               IF ( I.EQ.1 )
     $            WRKOPT = MAX( WRKOPT, INT( DWORK(IIP) ) + IIP - 1 )
C
               IF ( IP.GT.0 ) THEN
C
C                 Save the state matrix of the minimal part.
C                 Workspace: need   W3 = W2+N*(N+2).
C
                  CALL DLACPY( 'Full', IP, IP, DWORK(IAC), NCONT,
     $                         DWORK(IAS), IP )
C
C                 Compute the poles of the transfer function.
C                 Workspace: need   W3+N;
C                            prefer larger.
C
                  CALL DHSEQR( 'Eigenvalues', 'No vectors', IP, 1, IP,
     $                         DWORK(IAC), NCONT, DWORK(IRP),
     $                         DWORK(IIP), Z, 1, DWORK(JWORK1),
     $                         LDWORK-JWORK1+1, IERR )
                  IF ( IERR.NE.0 ) THEN
                     INFO = 2
                     RETURN
                  END IF
                  WRKOPT = MAX( WRKOPT,
     $                          INT( DWORK(JWORK1) ) + JWORK1 - 1 )
C
C                 Compute the zeros of the transfer function.
C
                  IPM1   = IP - 1
                  DIJNZ  = WITHD .AND. DIJ.NE.ZERO
                  FNDEIG = DIJNZ .OR. IPM1.GT.0
                  IF ( .NOT.FNDEIG ) THEN
                     IZ = 0
                  ELSE IF ( DIJNZ ) THEN
C
C                    Add the contribution due to D(i,j).
C                    Note that the matrix whose eigenvalues have to
C                    be computed remains in an upper Hessenberg form.
C
                     IZ = IP
                     CALL DLACPY( 'Full', IZ, IZ, DWORK(IAS), IP,
     $                            DWORK(IAC), NCONT )
                     CALL DAXPY( IZ, -DWORK(ICC)/DIJ, DWORK(IB), 1,
     $                           DWORK(IAC), NCONT )
                  ELSE
                     IF( TOL.LE.ZERO )
     $                  TOLDEF = EPSN*MAX( ANORM,
     $                                     DLANGE( 'Frobenius', IP, 1,
     $                                             DWORK(IB), 1, DWORK )
     $                                           )
C
                     DO 30 IM = 1, IPM1
                        IF ( ABS( DWORK(IB+IM-1) ).GT.TOLDEF ) GO TO 40
   30                CONTINUE
C
                     IZ = 0
                     GO TO 50
C
   40                CONTINUE
C
C                    Restore (part of) the saved state matrix.
C
                     IZ = IP - IM
                     CALL DLACPY( 'Full', IZ, IZ, DWORK(IAS+IM*(IP+1)),
     $                             IP, DWORK(IAC), NCONT )
C
C                    Apply the output injection.
C
                     CALL DAXPY( IZ, -DWORK(IAS+IM*(IP+1)-IP)/
     $                           DWORK(IB+IM-1), DWORK(IB+IM), 1,
     $                           DWORK(IAC), NCONT )
                  END IF
C
                  IF ( FNDEIG ) THEN
C
C                    Find the zeros.
C                    Workspace: need   W3+N;
C                               prefer larger.
C
                     CALL DHSEQR( 'Eigenvalues', 'No vectors', IZ, 1,
     $                            IZ, DWORK(IAC), NCONT, GN(K), GD(K),
     $                            Z, 1, DWORK(JWORK1), LDWORK-JWORK1+1,
     $                            IERR )
                     IF ( IERR.NE.0 ) THEN
                        INFO = 1
                        RETURN
                     END IF
                  END IF
C
C                 Compute the gain.
C
   50             CONTINUE
                  IF ( DIJNZ ) THEN
                     X = DIJ
                  ELSE
                     CALL TB04BX( IP, IZ, DWORK(IAS), IP, DWORK(ICC),
     $                            DWORK(IB), DIJ, DWORK(IRP),
     $                            DWORK(IIP), GN(K), GD(K), X, IWORK )
                  END IF
C
C                 Form the numerator coefficients in increasing or
C                 decreasing powers of the indeterminate.
C                 IAS is used here as pointer to the workspace.
C
                  IF ( ASCEND ) THEN
                     CALL MC01PD( IZ, GN(K), GD(K), DWORK(IB),
     $                            DWORK(IAS), IERR )
                  ELSE
                     CALL MC01PY( IZ, GN(K), GD(K), DWORK(IB),
     $                            DWORK(IAS), IERR )
                  END IF
                  JJ = K
C
                  DO 60 L = IB, IB + IZ
                     GN(JJ) = DWORK(L)*X
                     JJ = JJ + 1
   60             CONTINUE
C
C                 Form the denominator coefficients.
C
                  IF ( ASCEND ) THEN
                     CALL MC01PD( IP, DWORK(IRP), DWORK(IIP), GD(K),
     $                            DWORK(IAS), IERR )
                  ELSE
                     CALL MC01PY( IP, DWORK(IRP), DWORK(IIP), GD(K),
     $                            DWORK(IAS), IERR )
                  END IF
                  IGN(I,J) = IZ
                  IGD(I,J) = IP
               ELSE
C
C                 Null element.
C
                  IGN(I,J) = 0
                  IGD(I,J) = 0
                  GN(K) = DIJ
                  GD(K) = ONE
               END IF
C
            ELSE
C
C              Null element.
C
               IGN(I,J) = 0
               IGD(I,J) = 0
               GN(K) = DIJ
               GD(K) = ONE
            END IF
C
            K = K + MD
   70    CONTINUE
C
   80 CONTINUE
C
      RETURN
C *** Last line of TB04BD ***
      END


C Copyright(C) 1999-2020 National Technology & Engineering Solutions
C of Sandia, LLC (NTESS).  Under the terms of Contract DE-NA0003525 with
C NTESS, the U.S. Government retains certain rights in this software.
C
C See packages/seacas/LICENSE for details

C=======================================================================
      SUBROUTINE CUTROT (CUTPLA, CUTMAT, *)
C=======================================================================

C   --*** CUTROT *** (MESH) Get rotation matrix for cut
C   --   Written by Amy Gilkey - revised 07/03/86
C   --
C   --CUTROT makes up the rotation matrix that will rotate the mesh
C   --along the cutting plane.
C   --
C   --Parameters:
C   --   CUTPLA - IN - the 3 points defining the cutting plane;
C   --      CUTPLA(1,i) is the x coordinate for point i, etc
C   --   CUTMAT - OUT - the rotation matrix
C   --   * - return statement if the 3 points do not define a plane

      REAL CUTPLA(3,3)
      REAL CUTMAT(3,3)

      X1 = CUTPLA(1,1)
      Y1 = CUTPLA(2,1)
      Z1 = CUTPLA(3,1)
      X2 = CUTPLA(1,2)
      Y2 = CUTPLA(2,2)
      Z2 = CUTPLA(3,2)
      X3 = CUTPLA(1,3)
      Y3 = CUTPLA(2,3)
      Z3 = CUTPLA(3,3)

      X12 = SQRT ((X2-X1)**2 + (Y2-Y1)**2 + (Z2-Z1)**2)
      X13 = SQRT ((X3-X1)**2 + (Y3-Y1)**2 + (Z3-Z1)**2)
      X23 = SQRT ((X2-X3)**2 + (Y2-Y3)**2 + (Z2-Z3)**2)
      IF (X12 .EQ. 0.0) GOTO 100
      X15 = (X12**2 + X13**2 - X23**2) / (2.0*X12)
      XLAM = (X2-X1) / X12
      YLAM = (Y2-Y1) / X12
      ZLAM = (Z2-Z1) / X12
      X5 = X1 + XLAM*X15
      Y5 = Y1 + YLAM*X15
      Z5 = Z1 + ZLAM*X15
      X35 = SQRT ((X3-X5)**2 + (Y3-Y5)**2 + (Z3-Z5)**2)
      IF (X35 .EQ. 0.0) GOTO 100
      XPSI = (X3-X5) / X35
      YPSI = (Y3-Y5) / X35
      ZPSI = (Z3-Z5) / X35
      XNU = YLAM*ZPSI - ZLAM*YPSI
      YNU = ZLAM*XPSI - XLAM*ZPSI
      ZNU = XLAM*YPSI - YLAM*XPSI

      CUTMAT(1,1) = XLAM
      CUTMAT(2,1) = YLAM
      CUTMAT(3,1) = ZLAM
      CUTMAT(1,2) = XPSI
      CUTMAT(2,2) = YPSI
      CUTMAT(3,2) = ZPSI
      CUTMAT(1,3) = XNU
      CUTMAT(2,3) = YNU
      CUTMAT(3,3) = ZNU

      RETURN

  100 CONTINUE
      CALL PRTERR ('CMDERR', 'Points do not define a plane')
      RETURN 1
      END


C Head Simulation of disturbed phase screen
        program s_psc
*******************************************************************************
*
*       is a phase screen simulator which produces artifitial fringe
*       or speckle patterns and obtain integrated power spectrum or(/and)
*       optionally specified bispectral components.
*       The atmospheric disturbance is given by the Kolmogorov spectrum.
*       Seeing condition, aperture configuration and the object ( at this
*       point, only multiple point sources ) and the usage of the phase screen
*       must be specified by the user.
*
*       Written in standard FORTRAN and expected to run both on VMS and UNIX
*       simply by replacing FFT routines and random number generator.
*       On UNIX all tempory arrays are to be dynamically allocated by 'calloc()'
*       Source code should be called pscreen.for on VMS, pscreen.f on UNIX.
*       On a vector machine, effeciency of loops should be reconsidered.
*
*CMV No need to 'calloc()' arrays on Alliant
*
*       This is a re-organized version of the pscsn.for developed on Tacos.
*       The program is planned to be as users oriented as possible for the
*       future development of Optical/IR interferometry.
*
*       version 1.0    Dec 25, 1988 (Merry Christmas!)     Tadashi Nakajima
*       Today's resolution : `I will have real holidays next year and live
*                             a human life.'
*       version 2.0    Dec 30, 1988   Now sharp cut-offs of fluctuation power
*       spectrum at kapmin and kapmax are introduced to investigatate the
*       relative significance of the diffractive effect and refractive effect.
*       For instance, the refractive effect can be seen by setting lo = 0.1,
*       while the diffractive effect can be investigated by setting L0 = 0.4.
*
*       Changed to work with SCASIS/GHRIL interface, options to write
*       phase screen and detector plane.  Marco de Vos, 10-12-90
*
*       Optimised many nested loops by changing their order. Removed bug
*       in simulation of double source. Marco de Vos, may/jun 91
*
*******************************************************************************
*------ declarations ---------*
* Constants
        real      pi, rapdeg, rapmin, rapsec
        complex iu

* Integrated structure constant of the phase fluctuation
        real   LCn2

* Size of phase screen (m) and resolution (m/pix)
        real   Ls, Lp

* Size of phase screen (pix), size of power spectrum (pix)
        integer   Ns, Np
        parameter( Ns = 512  )
        parameter( Np = Ns/2 )

* Wave number, spatial frequencies and scale length of tubulence
        real k, kapmax, kapmin, L0, dkap, lo

* Wavelength, fractional bandwidth, # of samples
        real lambda, fraction
        integer nsamp

* Complex phase screen, real and imag are independent.
        complex psc(Ns,Ns)

* Power spectrum array
        real   pwr(Np,Np)

* Maximum # of components in source, maximum number of apertures
        integer   Maxcomp, Maxap
        parameter( Maxcomp = 50, Maxap = 15 )

* Source information (intensity and position in arcsec)
        real   s_int(Maxcomp), s_pos(Maxcomp,2)

* Aperture information (coordinate, radius, type)
        real   c_pos(Maxap,2), c_rad(Maxap)
        integer   c_typ(Maxap)

* Definition of subframes
        real   x_min, x_max, xstep, y_min, y_max, ystep

* Number of sources, apertures, frames
        integer ncomp, ncircle, nframe
*
        common/psc/ pi,rapdeg,rapmin,rapsec, iu,
     &              k, kapmax, kapmin, L0, LCn2, Ls, Lp,
     &              dkap, lambda, nframe,
     &              x_min, x_max, xstep, y_min, y_max, ystep,
     &              ncomp, ncircle, nsamp, fraction

* Work area for subroutine power
        real    image1(Np,Np), image2(Np,Np),  ary1(Np,Np), ary2(Np,Np)
        complex pdata1(Np,Np), pdata2(Np,Np), cary1(Np,Np), cary2(Np,Np)

* Variables local to main program
        real      zero
        integer   seed, n, iscreen, iframe, iz, jz, lx, ly
        logical   no_dist,w_screen,w_image,w_power
        character comment*80

C Head Initialisation, user input

* Common variables are not compatible with parameter statement.
        pi = 3.141592653
        rapdeg = pi/180.0
        rapmin = rapdeg/60.0
        rapsec = rapdeg/3600.0
        iu = (0.0,1.0)
        LCn2 = 5.0e-13
        Ls = 10.24
        Lp = 0.02

* Open log-file etc, get input parameters
        call init()

        nframe=50
        x_min=2.5
        y_min=2.5
        x_max=8.5
        y_max=8.5
        xstep=.15
        ystep=8.0
        call userint(nframe,1,1,'NFRAME=','Enter the number of frames')
        call userrpair(x_min,y_min,1,'SUB_MIN=',
     &        'Enter minimum X,Y of subframe (m)')
        call userrpair(x_max,y_max,1,'SUB_MAX=',
     &        'Enter maximum X,Y of subframe (m)')
        call userrpair(xstep,ystep,1,'SUB_STEP=',
     &        'Enter stepsize in X,Y (m)')

        L0=0.60
        lo=0.02
        call userrpair(L0,lo,2,'SCALE=',
     &                'Enter outer,inner scale of turbulence (m)')

        lambda=0.63e-6
        fraction=0.00476
        nsamp=3
        call userreal(lambda,1,2,'LAMBDA=','Enter wavelength (m)')
        call userreal(fraction,1,2,'FBAND=','Enter fractional bandwidth')
        call userint(nsamp,1,2,'NSAMP=','Enter number of sampling points')

        ncircle=1
        c_rad(1)=1.
        c_pos(1,1)=0.0
        c_pos(1,2)=0.0
        c_typ(1)=1
        call userint(ncircle,1,1,'NCIRCLE=','Enter number of apertures')
        do n=1,ncircle
           call userrpair(c_pos(n,1),c_pos(n,2),1,'CPOS=',
     &                   'Enter position in X,Y (m)')
           call userreal(c_rad(n),1,1,'CRAD=','Enter radius (m)')
           call userint(c_typ(n),1,1,'CTYPE=','Enter type (1=hole,0=void)')
           c_rad(n+1)=c_rad(n)
           c_pos(n+1,1)=c_pos(n,1)
           c_pos(n+1,2)=c_pos(n,1)
           c_typ(n+1)=c_typ(n)
           call cancel('CPOS=')
           call cancel('CRAD=')
           call cancel('CTYPE=')
        end do

        ncomp=1
        call userint(ncomp,1,1,'NCOMP=','Enter number of sources')
        do n=1,ncomp
           s_int(n)=1.
           s_pos(n,1)=0.0
           s_pos(n,2)=0.0
           call userrpair(s_pos(n,1),s_pos(n,2),1,'SPOS=',
     &                   'Enter position in X,Y (m)')
           call userreal(s_int(n),1,1,'SINT=','Enter intensity')
           call cancel('SPOS=')
           call cancel('SINT=')
        end do

        no_dist=.FALSE.
        w_screen=.FALSE.
        w_image=.FALSE.
        w_power=.TRUE.
        call userlog(no_dist,1,2,'NDIST=','No disturbance?')
        call userlog(w_screen,1,2,'WSCREEN=','Write screen data?')
        call userlog(w_image, 1,2,'WIMAGE=', 'Write image plane?')
        call userlog(w_power, 1,2,'WPOWER=', 'Write power spectrum?')

        seed=7597551
        call userint(seed,1,2,'SEED=','Enter seed for random generator')

* k: wave number, kap: spatial frequency in aperture plane

        k = 2.0 * pi / lambda

        if (Ls .lt. L0) then
          kapmin = 2.0 * pi / Ls
        else
          kapmin = 2.0 * pi / L0
        end if

        if (lo .lt. Lp) then
           kapmax = 2.0 * pi / Lp
        else
           kapmax = 2.0 * pi / lo
        end if

* dkap: one pixel in spatial frequency in aperture plane
        dkap = 2.0 * pi / Ls

C Head Loop over frames

*
        write(1,*) 'frm_per_scr ',
     1     (int((x_max-x_min)/xstep)+1)*(int((y_max-y_min)/ystep)+1)
C JLP 91        call easyou(0)

        iframe=1
        iscreen=1
*------------------------------------
287     if ( no_dist ) then
*            no atmospheric disturbance
             do iz=1, Ns
               do jz=1, Ns
                 psc(iz,jz) = (0.0,0.0)
               end do
             end do
           else
* Create a complex phase screen
             write(comment,310) iscreen
  310        format(' Generating ',i3,'th complex phase screen')
             call  screen(psc, Ns, seed)
             iscreen=iscreen+1
             if (w_screen)
     &           call psc_wscreen(psc,Ns,Ns,pdata1,pdata2,Np,Np)
           end if

* Sample phase screen, form images, calculate power
           call  power(psc, Ns, pwr, Np, iframe, Maxcomp, Maxap,
     1            s_int, s_pos, c_pos, c_rad, c_typ, pdata1, pdata2,
     2            image1, image2, ary1, ary2, cary1, cary2)
           if (w_image) call psc_wimage(ary1,ary2,Np,Np)
*
        if(iframe.le.nframe)goto 287
*-----------------------------------------
*

* Write normalized integrated power spectrum

        zero = pwr(Np/2+1,Np/2+1)
        if (zero.eq.0.0) zero=1.0
        write(1,*) 'Scaling: divide by ',zero
C JLP 91        call easyou(0)
* CMV REV
        do ly=1, Np
          do lx=1, Np
            pwr(lx,ly) = pwr(lx,ly)/zero
          end do
        end do
        print *,'Scaled'
        if (w_power) call psc_wpower(pwr,Np,Np)
*
        n=0
        call data_close(1,n)
        n=0
        call data_close(2,n)
        n=0
        call data_close(3,n)
        call finis()

        end

C Head Compute phase screen

        subroutine screen(psc, Ns, seed)
***************************************************************************
* Creates complex phase screen
***************************************************************************
        implicit  none
        integer Ns,seed
        complex psc(Ns,Ns)

        real    pi,rapdeg,rapmin,rapsec,Ls,Lp
        complex iu
        real    k, kapmax, kapmin, L0, LCn2, dkap, lambda, fraction
        real    x_min,x_max,xstep, y_min,y_max,ystep
        integer ncomp,ncircle,nsamp,nframe
*
        common/psc/ pi,rapdeg,rapmin,rapsec, iu,
     &              k, kapmax, kapmin, L0, LCn2, Ls, Lp,
     &              dkap, lambda, nframe,
     &              x_min, x_max, xstep, y_min, y_max, ystep,
     &              ncomp, ncircle, nsamp, fraction

* Radial spatial frequency in aperture plane
        real   kap

* Dimension array for fft, origin of the phase screen
        integer   N(2), Nc

        integer   i,j,jx,jy
        real      Fs,Fs0,atten,radi,u,ran
        character comment*80

*------ generation of random phase in spatial domain -------------
* Complex Gaussian random numbers are generated by
* the inverse transform method.

        N(1) = Ns
        N(2) = Ns
        call four2(psc,Ns,Ns,0)
        Nc = Ns/2+1
        Fs0 = 0.033*pi*LCn2*k*k*2.0*dkap*dkap
* CMV REV
        do 100 jy=1, Ns

          if (mod(jy,100).eq.0) then
             write(comment,*) 'Making line ',jy
             call status(comment)
          end if

          do 110 jx=1, Ns

            kap = dkap*sqrt( float( (jx-Nc)**2+(jy-Nc)**2 ) )

* Introducing smooth cut-offs below kapmin and beyond kapmax.

            if(kap .gt. kapmax) atten = exp(-(kap-kapmax)/(5.0*dkap))
            if(kap .lt. kapmin) atten = exp(-(kapmin-kap)/(5.0*dkap))
            if(kap.le.kapmax .and. kap.ge.kapmin) atten = 1.0

* kap**(-11/3) is undefined at kap=0.
            if(kap .eq. 0.0) kap = dkap

            u=0.0
288           u=ran(seed)
              if(u.le.0.0) goto 288

            radi = sqrt(-2.0*log(u))
            Fs = Fs0*kap**(-11.0/3.0)
            psc(jx,jy) = atten*sqrt(Fs)*radi*cexp(iu*2.0*pi*ran(seed))

  110     continue
  100   continue

C Page

*---------- Fourier transform ------------------------------
* Shift origin of Fourier space to (Nc,Nc)
* CMV REV
        do j=1,Ns,2
          do i=1,Ns,2
            psc(i+1,j) = -psc(i+1,j)
            psc(i,j+1) = -psc(i,j+1)
          end do
        end do

        write(comment,555) N(1),N(2)
  555   format(' ', 'start ',i3,'x',i3,' sized fft              ')
        call four2(psc,Ns,Ns,-1)
        write(comment,666)
  666   format(' ', 'end fft for screen                         ')

* Now psc is a complex random phase screen
* to avoid discontinuity shift origin of phase screen to (Nc,Nc)
* CMV REV
        do j=1,Ns,2
          do i=1,Ns,2
            psc(i+1,j) = -psc(i+1,j)
            psc(i,j+1) = -psc(i,j+1)
          end do
        end do

        return
        end

C Head Integrate power spectrum

        subroutine power(psc, Ns, pwr, Np, iframe, Maxcomp, Maxap,
     1          s_int, s_pos, c_pos, c_rad, c_typ, pdata1, pdata2,
     2                   image1, image2, ary1, ary2, cary1, cary2)
*******************************************************************************
* Produces fringe/speckle patterns of multiple point sources.
* Finite bandwidth effect is taken into account.
*******************************************************************************

        implicit none
        integer Ns,Np,iframe,Maxcomp,Maxap

* Source information (intensity and position in arcsec)
        real   s_int(Maxcomp), s_pos(Maxcomp,2)

* Aperture information (coordinate, radius, type)
        real   c_pos(Maxap,2), c_rad(Maxap)
        integer   c_typ(Maxap)

* Phase screen
        complex psc(Ns,Ns)

* Power spectrum
        real   pwr(Np,Np)

* Temporary arrays
        real    image1(Np,Np), image2(Np,Np),  ary1(Np,Np), ary2(Np,Np)
        complex pdata1(Np,Np), pdata2(Np,Np), cary1(Np,Np), cary2(Np,Np)

* Common block
        real    pi,rapdeg,rapmin,rapsec, Ls, Lp
        complex iu
        real    k, kapmax, kapmin, L0, LCn2, dkap, lambda, fraction
        real    x_min,x_max,xstep, y_min,y_max,ystep
        integer ncomp,ncircle,nsamp,nframe
*
        common/psc/ pi,rapdeg,rapmin,rapsec, iu,
     &              k, kapmax, kapmin, L0, LCn2, Ls, Lp,
     &              dkap, lambda, nframe,
     &              x_min, x_max, xstep, y_min, y_max, ystep,
     &              ncomp, ncircle, nsamp, fraction

*
        integer N(2),Wc,Npx
        real    lpix,Scale,factor
        real    x,y,xa,ya,ox,oy
        integer ix,iy,jx,jy,lx,ly,dx,dy,iox,ioy
        integer loop1,loop2,i
        character comment*80

        N(1) = Np
        N(2) = Np
        call four2(pdata1,Np,Np,0)
        lpix = Ls/Ns
        Wc = Np/2+1
        Scale = Ls/lambda*Np/Ns*rapsec

* Setting apertures on Np sized window in Ns sized complex phase screen

        do 10 x=x_min,x_max,xstep
        do 20 y=y_min,y_max,ystep
**************************** major loop ***************************************

           write(1,111) x,y
C JLP 91        call easyou(0)
  111      format(' ',' creating a pair of speckle/fringe patterns',1x,
     1            'xy',f10.3,1x,f10.3)

* Reset temporary arrays
* CMV REV
           do jy=1,Np
             do jx=1,Np
               image1(jx,jy) = 0.0
               image2(jx,jy) = 0.0
                 ary1(jx,jy) = 0.0
                 ary2(jx,jy) = 0.0
                cary1(jx,jy) = (0.0,0.0)
                cary2(jx,jy) = (0.0,0.0)
             end do
           end do

C Head Loop over waveband

           do 100 loop1=1,nsamp

* Reset pdata1 and pdata2
* CMV REV
             do iy=1, Np
               do ix=1, Np
                 pdata1(ix,iy) = (0.0,0.0)
                 pdata2(ix,iy) = (0.0,0.0)
               end do
             end do

**************** intermediate loop *****************
* nsamp data points within the bandpass
             factor =  1.0 + fraction*float(loop1 - nsamp/2)

             npx = 0

             do 30 i=1,ncircle
*-----------------------------------------
* Filling pdata with aperture phases

            do 40 xa = x+c_pos(i,1)-c_rad(i), x+c_pos(i,1)+c_rad(i), lpix
            do 50 ya = y+c_pos(i,2)-c_rad(i), y+c_pos(i,2)+c_rad(i), lpix
              ox = xa-x-c_pos(i,1)
              oy = ya-y-c_pos(i,2)
              if( ox*ox+oy*oy .lt. c_rad(i)*c_rad(i) ) then
                ix = nint( (xa-x)/lpix )+Wc
                iy = nint( (ya-y)/lpix )+Wc

* Shift origin of the window to (Wc,Wc)
                lx = nint(xa/lpix)
                ly = nint(ya/lpix)

                if( c_typ(i) .eq. 1) npx = npx+1
                if( c_typ(i) .eq. 0) npx = npx-1

                if( npx.eq.1 .or. mod(npx,100) .eq. 0 ) then
                  write(comment,777) npx
                  call status(comment)
                end if
  777   format('# of  positive pixels',1x,i5)

                if( mod(ix+iy,2) .eq. 0 ) then
                  pdata1(ix,iy) = cexp( iu*factor*real(psc(lx,ly)) )*c_typ(i)
                  pdata2(ix,iy) = cexp(iu*factor*aimag(psc(lx,ly)) )*c_typ(i)
                else
                  pdata1(ix,iy) =-cexp( iu*factor*real(psc(lx,ly)) )*c_typ(i)
                  pdata2(ix,iy) =-cexp(iu*factor*aimag(psc(lx,ly)) )*c_typ(i)
                end if

              end if
   50       continue
   40       continue
*-----------------------------------------
   30     continue

* Simulate propagation from aperture plane to detector plane

        call four2(pdata1,Np,Np,-1)
        call four2(pdata2,Np,Np,-1)

* Form speckle/fringe patterns
* CMV REV
        do iy=1, Np
          do ix=1, Np
            image1(ix,iy) = image1(ix,iy) +
     1                      real( pdata1(ix,iy)*conjg(pdata1(ix,iy)) )
            image2(ix,iy) = image2(ix,iy) +
     2                      real( pdata2(ix,iy)*conjg(pdata2(ix,iy)) )
          end do
        end do

******************* end of loop1 ********************
  100   continue

C Head Put in sources and make power spectrum

        do 60 loop2=1,ncomp
*--------------------------------------------
*       ncomp components in source structure
          dx = int(Scale * s_pos(loop2,1))
          dy = int(Scale * s_pos(loop2,2))

          iox=0
          ioy=0
          if (dx.lt.0) iox=dx
          if (dy.lt.0) ioy=dy
* CMV REV
          do iy=1-ioy,Np-dy+ioy
          do ix=1-iox,Np-dx+iox
            ary1(ix+dx,iy+dy) = ary1(ix,iy)+s_int(loop2)*image1(ix,iy)
            ary2(ix+dx,iy+dy) = ary2(ix,iy)+s_int(loop2)*image2(ix,iy)
          end do
          end do
*-------------------------------------------
   60   continue

        write(comment,320) iframe

* Shift origin of the image to (Wc,Wc) before fourier transform
* CMV REV
        do iy=1,Np,2
        do ix=1,Np,2
          cary1(ix,iy)     = ary1(ix,iy)
          cary1(ix+1,iy+1) = ary1(ix+1,iy+1)
          cary1(ix+1,iy)   =-ary1(ix+1,iy)
          cary1(ix,iy+1)   =-ary1(ix,iy+1)
        end do
        end do

        call four2(cary1,Np,Np,-1)

* CMV REV
        do jy=1,Np
        do jx=1,Np
          pwr(jx,jy) = pwr(jx,jy) + real(cary1(jx,jy)*conjg(cary1(jx,jy)))
        end do
        end do

        iframe = iframe + 1
        if( iframe .gt. nframe ) return

        write(comment,320) iframe
  320   format(' ', 'processing ',i4,' th frame                    ')

* Shift origin of the image to (Wc,Wc) before fourier transform
* CMV REV
        do iy=1,Np,2
        do ix=1,Np,2
          cary2(ix,iy)     = ary2(ix,iy)
          cary2(ix+1,iy+1) = ary2(ix+1,iy+1)
          cary2(ix+1,iy)   =-ary2(ix+1,iy)
          cary2(ix,iy+1)   =-ary2(ix,iy+1)
        end do
        end do

        call four2(cary2,Np,Np,-1)
*CMV REV
        do jy=1,Np
        do jx=1,Np
          pwr(jx,jy) = pwr(jx,jy) + real(cary2(jx,jy)*conjg(cary2(jx,jy)))
        end do
        end do

        iframe = iframe + 1
        if( iframe .gt. nframe) return

************************* end of major loop ***********************************
   20   continue
   10   continue

        end

CHead Subroutine to write phase screen

        subroutine psc_wscreen(array,dx,dy,rbuf,ibuf,nx,ny)
c
        implicit  none
        integer   dx,dy,nx,ny
        complex   array(dx,dy)
        real      rbuf(nx,ny),ibuf(nx,ny)
        character ofile*25,otype*10,oform*5
        integer   err,ix,iy,x0,y0
        logical   first
        data      first/.TRUE./
        save      first
c
        err=0
        if (first) then
           first=.false.
           ofile=' '
           otype='GHRIL'
           oform='-32'
           call usertext(ofile,1,'SFILE=',
     &                  'Enter filename for phase-screen')
           call usertext(otype,2,'STYPE=','Enter file-type')
           call usertext(oform,2,'SFORM=','Enter format for GHRIL-file')
           call data_open(1,ofile,otype,'WRITE',err)
           call data_wkey(1,'OBJECT','Screen',' ',err)
           call date_header(1,'DATE','TIME')
        end if
*
*       Write each screen in eight parts: re,im ...
*
        do x0=0,dx-1,nx
          do y0=0,dy-1,nx
*CMV REV
             do iy=1,ny
               do ix=1,nx
                  rbuf(ix,iy)= real(array(x0+ix,y0+iy))
                  ibuf(ix,iy)=aimag(array(x0+ix,y0+iy))
               end do
             end do
             call data_write(1,0,0,1,nx,ny,rbuf,0,0,nx,oform,err)
             call data_write(1,0,0,1,nx,ny,ibuf,0,0,nx,oform,err)
          end do
        end do
        call data_error(err)
        return
        end

CHead Subroutine to write image plane

        subroutine psc_wimage(array1,array2,dx,dy)
c
        implicit  none
        real      array1(*),array2(*)
        integer   dx,dy
        character ofile*25,otype*10,oform*5
        integer   err
        logical   first
        data      first/.TRUE./
        save      first
c
        err=0
        if (first) then
           first=.false.
           ofile=' '
           otype='GHRIL'
           oform='-32'
           call usertext(ofile,1,'IFILE=',
     &                  'Enter filename for image-plane')
           call usertext(otype,2,'ITYPE=','Enter file-type')
           call usertext(oform,2,'SFORM=','Enter format for GHRIL-file')
           call data_open(2,ofile,otype,'WRITE',err)
           call data_wkey(2,'OBJECT','Image',' ',err)
           call date_header(2,'DATE','TIME')
        end if
        call data_write(2,0,0,1,dx,dy,array1,0,0,dx,oform,err)
        call data_write(2,0,0,1,dx,dy,array2,0,0,dx,oform,err)
        call data_error(err)
        return
        end

CHead Subroutine to write phase screen

        subroutine psc_wpower(array,dx,dy)
c
        implicit  none
        real      array(*)
        integer   dx,dy
        character ofile*25,otype*10,oform*5
        integer   err
        logical   first
        data      first/.TRUE./
        save      first
c
        err=0
        if (first) then
           first=.false.
           ofile=' '
           otype='GHRIL'
           oform='-32'
           call usertext(ofile,1,'PFILE=',
     &                  'Enter filename for power spectrum')
           call usertext(otype,2,'PTYPE=','Enter file-type')
           call usertext(oform,2,'SFORM=','Enter format for GHRIL-file')
           call data_open(3,ofile,otype,'WRITE',err)
           call data_wkey(3,'OBJECT','Power',' ',err)
           call date_header(3,'DATE','TIME')
        end if
        call data_write(3,0,0,1,dx,dy,array,0,0,dx,oform,err)
        call data_error(err)
        return
        end


      SUBROUTINE MB03VY( N, P, ILO, IHI, A, LDA1, LDA2, TAU, LDTAU,
     $                   DWORK, LDWORK, INFO )
C
C     SLICOT RELEASE 5.7.
C
C     Copyright (c) 2002-2020 NICONET e.V.
C
C     PURPOSE
C
C     To generate the real orthogonal matrices Q_1, Q_2, ..., Q_p,
C     which are defined as the product of ihi-ilo elementary reflectors
C     of order n, as returned by SLICOT Library routine MB03VD:
C
C        Q_j = H_j(ilo) H_j(ilo+1) . . . H_j(ihi-1).
C
C     ARGUMENTS
C
C     Input/Output Parameters
C
C     N       (input) INTEGER
C             The order of the matrices Q_1, Q_2, ..., Q_p.  N >= 0.
C
C     P       (input) INTEGER
C             The number p of transformation matrices.  P >= 1.
C
C     ILO     (input) INTEGER
C     IHI     (input) INTEGER
C             The values of the indices ilo and ihi, respectively, used
C             in the previous call of the SLICOT Library routine MB03VD.
C             1 <= ILO <= max(1,N); min(ILO,N) <= IHI <= N.
C
C     A       (input/output) DOUBLE PRECISION array, dimension
C             (LDA1,LDA2,N)
C             On entry, the leading N-by-N strictly lower triangular
C             part of A(*,*,j) must contain the vectors which define the
C             elementary reflectors used for reducing A_j, as returned
C             by SLICOT Library routine MB03VD, j = 1, ..., p.
C             On exit, the leading N-by-N part of A(*,*,j) contains the
C             N-by-N orthogonal matrix Q_j, j = 1, ..., p.
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     TAU     (input) DOUBLE PRECISION array, dimension (LDTAU,P)
C             The leading N-1 elements in the j-th column must contain
C             the scalar factors of the elementary reflectors used to
C             form the matrix Q_j, as returned by SLICOT Library routine
C             MB03VD.
C
C     LDTAU   INTEGER
C             The leading dimension of the array TAU.
C             LDTAU >= max(1,N-1).
C
C     Workspace
C
C     DWORK   DOUBLE PRECISION array, dimension (LDWORK)
C             On exit, if INFO = 0, DWORK(1) returns the optimal value
C             of LDWORK.
C
C     LDWORK  INTEGER
C             The length of the array DWORK.  LDWORK >= MAX(1,N).
C             For optimum performance LDWORK should be larger.
C
C             If LDWORK = -1, then a workspace query is assumed;
C             the routine only calculates the optimal size of the
C             DWORK array, returns this value as the first entry of
C             the DWORK array, and no error message related to LDWORK
C             is issued by XERBLA.
C
C     Error Indicator
C
C     INFO    INTEGER
C             = 0:  successful exit;
C             < 0:  if INFO = -i, the i-th argument had an illegal
C                   value.
C
C     METHOD
C
C     Each matrix Q_j is generated as the product of the elementary
C     reflectors used for reducing A_j. Standard LAPACK routines for
C     Hessenberg and QR decompositions are used.
C
C     REFERENCES
C
C     [1] Bojanczyk, A.W., Golub, G. and Van Dooren, P.
C         The periodic Schur decomposition: algorithms and applications.
C         Proc. of the SPIE Conference (F.T. Luk, Ed.), 1770, pp. 31-42,
C         1992.
C
C     [2] Sreedhar, J. and Van Dooren, P.
C         Periodic Schur form and some matrix equations.
C         Proc. of the Symposium on the Mathematical Theory of Networks
C         and Systems (MTNS'93), Regensburg, Germany (U. Helmke,
C         R. Mennicken and J. Saurer, Eds.), Vol. 1, pp. 339-362, 1994.
C
C     NUMERICAL ASPECTS
C
C     The algorithm is numerically stable.
C
C     CONTRIBUTOR
C
C     V. Sima, Katholieke Univ. Leuven, Belgium, and A. Varga,
C     German Aerospace Center, DLR Oberpfaffenhofen, February 1999.
C     Partly based on the routine PSHTR by A. Varga
C     (DLR Oberpfaffenhofen), November 26, 1995.
C
C     REVISIONS
C
C     V. Sima, Research Institute for Informatics, Bucharest, Feb. 2004,
C     July 2012.
C
C     KEYWORDS
C
C     Hessenberg form, orthogonal transformation, periodic systems,
C     similarity transformation, triangular form.
C
C     ******************************************************************
C
C     .. Parameters ..
      DOUBLE PRECISION  ZERO, ONE
      PARAMETER         ( ZERO = 0.0D+0, ONE = 1.0D+0 )
C
C     .. Scalar Arguments ..
      INTEGER           IHI, ILO, INFO, LDA1, LDA2, LDTAU, LDWORK, N, P
C     ..
C     .. Array Arguments ..
      DOUBLE PRECISION  A( LDA1, LDA2, * ), DWORK( * ), TAU( LDTAU, * )
C     ..
C     .. Local Scalars ..
      LOGICAL           LQUERY
      INTEGER           J, NH, WRKOPT
C     ..
C     .. External Subroutines ..
      EXTERNAL          DLASET, DORGHR, DORGQR, XERBLA
C     ..
C     .. Intrinsic Functions ..
      INTRINSIC         INT, MAX, MIN
C     ..
C     .. Executable Statements ..
C
C     Test the input scalar arguments.
C
      INFO = 0
      IF( N.LT.0 ) THEN
         INFO = -1
      ELSE IF( P.LT.1 ) THEN
         INFO = -2
      ELSE IF( ILO.LT.1 .OR. ILO.GT.MAX( 1, N ) ) THEN
         INFO = -3
      ELSE IF( IHI.LT.MIN( ILO, N ) .OR. IHI.GT.N ) THEN
         INFO = -4
      ELSE IF( LDA1.LT.MAX( 1, N ) ) THEN
         INFO = -6
      ELSE IF( LDA2.LT.MAX( 1, N ) ) THEN
         INFO = -7
      ELSE IF( LDTAU.LT.MAX( 1, N-1 ) ) THEN
         INFO = -9
      ELSE
         NH = IHI - ILO + 1
         LQUERY = LDWORK.EQ.-1
         IF( LQUERY ) THEN
            CALL DORGHR( N, ILO, IHI, A, LDA1, TAU, DWORK, -1, INFO )
            WRKOPT = MAX( 1, N, INT( DWORK( 1 ) ) )
            IF ( NH.GT.1 ) THEN
               CALL DORGQR( NH, NH, NH-1, A, LDA1, TAU, DWORK, -1, INFO)
               WRKOPT = MAX( WRKOPT, INT( DWORK( 1 ) ) )
            END IF
         END IF
         IF( LDWORK.LT.MAX( 1, N ) .AND. .NOT. LQUERY )
     $      INFO = -11
      END IF
      IF( INFO.NE.0 ) THEN
         CALL XERBLA( 'MB03VY', -INFO )
         RETURN
      END IF
C
C     Quick return if possible.
C
      IF ( N.EQ.0 ) THEN
         DWORK( 1 ) = ONE
         RETURN
      ELSE IF( LQUERY ) THEN
         DWORK( 1 ) = WRKOPT
         RETURN
      END IF
C
C     Generate the orthogonal matrix Q_1.
C
      CALL DORGHR( N, ILO, IHI, A, LDA1, TAU, DWORK, LDWORK, INFO )
      WRKOPT = INT( DWORK( 1 ) )
C
      DO 20 J = 2, P
C
C        Generate the orthogonal matrix Q_j.
C        Set the first ILO-1 and the last N-IHI rows and columns of Q_j
C        to those of the unit matrix.
C
         CALL DLASET( 'Full', N, ILO-1, ZERO, ONE, A( 1, 1, J ), LDA1 )
         CALL DLASET( 'Full', ILO-1, NH, ZERO, ZERO, A( 1, ILO, J ),
     $                LDA1 )
         IF ( NH.GT.1 )
     $      CALL DORGQR( NH, NH, NH-1, A( ILO, ILO, J ), LDA1,
     $                   TAU( ILO, J ), DWORK, LDWORK, INFO )
         IF ( IHI.LT.N ) THEN
            CALL DLASET( 'Full', N-IHI, NH, ZERO, ZERO,
     $                   A( IHI+1, ILO, J ), LDA1 )
            CALL DLASET( 'Full', IHI, N-IHI, ZERO, ZERO,
     $                   A( 1, IHI+1, J ), LDA1 )
            CALL DLASET( 'Full', N-IHI, N-IHI, ZERO, ONE,
     $                   A( IHI+1, IHI+1, J ), LDA1 )
         END IF
   20 CONTINUE
C
      DWORK( 1 ) = MAX( WRKOPT, INT( DWORK( 1 ) ) )
      RETURN
C
C *** Last line of MB03VY ***
      END


module EQS_EDROMO
! Description:
!    Contains the subroutine for computing the RHS of the EDromo equations in
!    double precision.
!
! Author:
!    Davide Amato
!    Space Dynamics Group - Technical University of Madrid
!    d.amato@upm.es
!
! ==============================================================================

use KINDS, only: ik,dk,qk
implicit none

contains

subroutine DEDROMO_RHS(neq,phi,z,zdot)
! Description:
!    Computes the value of the right-hand side of the equations of motion of the
!    EDromo formulation.
!
! ==============================================================================

! MODULES
use AUXILIARIES, only: inSoI
use SETTINGS,    only: flag_time_EDr
use TRANSFORM,   only: DINERT2ORB_EDROMO
use THIRDBODIES, only: DACC3B_ND
use CONSTANTS,   only: DU,TU,wEarth,smaEarth,muEarth,muSun

! VARIABLES
implicit none

! Arguments
integer(ik),intent(in)     ::  neq             ! Number of equations
real(dk),intent(in)        ::  phi             ! EDromo independent variable
real(dk),intent(in)        ::  z(1:neq)        ! EDromo state vector, ND
real(dk),intent(out)       ::  zdot(1:neq)     ! RHS of EoM's, ND

! Auxiliary quantities
real(dk)    ::  sph,cph
real(dk)    ::  rho,rmag,zeta,emme
real(dk)    ::  cnu,snu
real(dk)    ::  enne,L3,wz
real(dk)    ::  aux0,aux1,aux2,aux3,aux4

! State
real(dk)    ::  rV(1:3),t
real(dk)    ::  x_vec(1:3),y_vec(1:3)

! Potential
real(dk)    ::  Upot,dUdt,dUdr(1:3)

! Perturbations
real(dk)    ::  lon_Earth,r2(1:3)
real(dk)    ::  f(1:3),p(1:3)

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

! INDEX OF STATE VECTORS

! z(1) = zeta1
! z(2) = zeta2
! z(3) = zeta3
! z(4) = zeta4
! z(5) = zeta5
! z(6) = zeta6
! z(7) = zeta7
! z(8) = zeta8 (physical time,
!                      constant time element,
!                      linear time element,
!                      depending on flag_time)

! ==============================================================================
! 01. COMPUTE AUXILIARY QUANTITIES (1)
! ==============================================================================

! Store trig functions
sph = sin(phi)
cph = cos(phi)

rho   = 1._dk - z(1)*cph - z(2)*sph
rmag  = z(3)*rho
zeta  = z(1)*sph - z(2)*cph
emme  = sqrt(1._dk - z(1)**2_ik - z(2)**2_ik)

cnu = (cph - z(1) + (zeta*z(2))/(emme + 1._dk))/rho
snu = (sph - z(2) - (zeta*z(1))/(emme + 1._dk))/rho

! ==============================================================================
! 02. COMPUTE POSITION IN INERTIAL FRAME AND POTENTIAL
! ==============================================================================

! Intermediate frame unit vectors
x_vec = 2._dk*[ .5_dk - z(5)**2_ik - z(6)**2_ik,  &
           &  z(4)*z(5) + z(6)*z(7),  &
           &  z(4)*z(6) - z(5)*z(7)]
y_vec = 2._dk*[  z(4)*z(5) - z(6)*z(7),  &
           & .5_dk - z(4)**2_ik - z(6)**2_ik,  &
           &  z(5)*z(6) + z(4)*z(7)]

! Position in inertial frame
rV = rmag*(x_vec*cnu + y_vec*snu)

! Potential
Upot = 0._dk; dUdt = 0._dk; dUdr = 0._dk

!! Velocity in inertial frame
!i_vec = x_vec*cnu + y_vec*snu
!j_vec = y_vec*cnu - x_vec*snu
!v_rad = zeta/(sqrt(z(3))*rho)
!v_tan = sqrt((1. - z(1)**2 - z(2)**2)/(z(3)*rho**2) - 2.*Upot)
!vV = v_rad*i_vec + v_tan*j_vec

! ==============================================================================
! 03. COMPUTE PERTURBING ACCELERATIONS
! ==============================================================================

p = 0._dk; f = 0._dk

! Get time
if ( flag_time_EDr == 0_ik ) then
    ! Physical time
    t = z(8)
elseif  ( flag_time_EDr == 1_ik ) then
    ! Constant time element
    t = z(8) - z(3)**1.5_dk*(zeta - phi)
elseif  ( flag_time_EDr == 2_ik ) then
    ! Linear time element
    t = z(8) - z(3)**1.5_dk*zeta
end if

lon_Earth = wEarth*(t/TU)
if (inSoI) then
    ! Third body: Sun
    r2 = -(smaEarth/DU)*[cos(lon_Earth),sin(lon_Earth),0._dk]
    p  = DACC3B_ND(rV,r2,muEarth,muSun)
else
    ! Third body: Earth
    r2 = (smaEarth/DU)*[cos(lon_Earth),sin(lon_Earth),0._dk]
    p  = DACC3B_ND(rV,r2,muSun,muEarth)
end if

! Inertial -> Orbital frame
p    = DINERT2ORB_EDROMO(p,z,cnu,snu)
!dUdr = DINERT2ORB_EDROMO(dUdr,z,cnu,snu)

f = p + dUdr

! ==============================================================================
! 04. COMPUTE AUXILIARY QUANTITIES (2)
! ==============================================================================

enne = sqrt(emme**2_ik - 2._dk*z(3)*rho**2_ik*Upot)

aux0 = (p(1)*zeta + p(2)*enne + dUdt*sqrt(z(3))*rho)
zdot(3) = 2._dk*z(3)**3_ik*aux0
L3 = zdot(3)/(2._dk*z(3))

aux1 = ((2._dk*Upot - f(1)*rmag)*(2._dk - rho + emme)*rmag)/(emme*(emme+1._dk))
aux2 = (L3*zeta*(rho-emme))/(emme*(emme+1._dk))
wz   = (enne - emme)/rho + aux1 + aux2

! ==============================================================================
! 05. COMPUTE RIGHT-HAND SIDE
! ==============================================================================

! In-plane
aux3 = (f(1)*rmag - 2._dk*Upot)*rmag

zdot(1) =  aux3*sph + L3*((1._dk+rho)*cph - z(1))
zdot(2) = -aux3*cph + L3*((1._dk+rho)*sph - z(2))

! Out-of-plane
aux4 = (f(3)*rmag**2_ik)/(2._dk*enne)

zdot(4) =  aux4*(z(7)*cnu - z(6)*snu) + 0.5_dk*wz*z(5)
zdot(5) =  aux4*(z(6)*cnu + z(7)*snu) - 0.5_dk*wz*z(4)
zdot(6) = -aux4*(z(5)*cnu - z(4)*snu) + 0.5_dk*wz*z(7)
zdot(7) = -aux4*(z(4)*cnu + z(5)*snu) - 0.5_dk*wz*z(6)

! Time / Time Element
if (flag_time_EDr == 0_ik) then
    zdot(8) = sqrt(z(3))*rmag
else if (flag_time_EDr == 1_ik) then   ! Constant Time Element
    zdot(8) = z(3)**1.5_dk * ( aux3 + (zeta - 1.5_dk*phi)*zdot(3)/z(3) )
else if (flag_time_EDr == 2_ik) then   ! Linear Time Element
    zdot(8) = z(3)**1.5_dk * ( 1._dk + aux3 + 2._dk*L3*zeta )
end if

end subroutine DEDROMO_RHS

end module EQS_EDROMO


!
!
!                             AMG4PSBLAS version 1.0
!    Algebraic Multigrid Package
!               based on PSBLAS (Parallel Sparse BLAS version 3.7)
!
!    (C) Copyright 2021
!
!        Salvatore Filippone
!        Pasqua D'Ambra
!        Fabio Durastante
!
!    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.
!      3. The name of the AMG4PSBLAS group or the names of its contributors may
!         not be used to endorse or promote products derived from this
!         software without specific written permission.
!
!    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 AMG4PSBLAS GROUP OR ITS 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.
!
!
! File: amg_caggrmat_minnrg_bld.F90
!
! Subroutine: amg_caggrmat_minnrg_bld
! Version:    complex
!
!  This routine builds a coarse-level matrix A_C from a fine-level matrix A
!  by using the Galerkin approach, i.e.
!
!                               A_C = P_C^T A P_C,
!
!  where P_C is a prolongator from the coarse level to the fine one.
!
!  The prolongator P_C is built according to a smoothed aggregation algorithm,
!  i.e. it is obtained by applying a damped Jacobi smoother to the piecewise
!  constant interpolation operator P corresponding to the fine-to-coarse level
!  mapping built by the amg_aggrmap_bld subroutine:
!
!                            P_C = (I - omega*D^(-1)A) * P,
!
!  where D is the diagonal matrix with main diagonal equal to the main diagonal
!  of A, and omega is a suitable smoothing parameter. An estimate of the spectral
!  radius of D^(-1)A, to be used in the computation of omega, is provided,
!  according to the value of p%parms%aggr_omega_alg, specified by the user
!  through amg_cprecinit and amg_cprecset.
!  4. Minimum energy aggregation:
!    M. Sala, R. Tuminaro: A new Petrov-Galerkin smoothed aggregation preconditioner
!    for nonsymmetric linear systems, SIAM J. Sci. Comput., 31(1):143-166 (2008)
!
!  On output from this routine the entries of AC, op_prol, op_restr
!  are still in "global numbering" mode; this is fixed in the calling routine
!  aggregator%mat_bld.
!
!
! Arguments:
!    a          -  type(psb_cspmat_type), input.
!                  The sparse matrix structure containing the local part of
!                  the fine-level matrix.
!    desc_a     -  type(psb_desc_type), input.
!                  The communication descriptor of the fine-level matrix.
!    p          -  type(amg_c_onelev_type), input/output.
!                  The 'one-level' data structure that will contain the local
!                  part of the matrix to be built as well as the information
!                  concerning the prolongator and its transpose.
!    parms      -   type(amg_sml_parms), input
!                  Parameters controlling the choice of algorithm
!    ac         -  type(psb_cspmat_type), output
!                  The coarse matrix on output
!
!    ilaggr     -  integer, dimension(:), input
!                  The mapping between the row indices of the coarse-level
!                  matrix and the row indices of the fine-level matrix.
!                  ilaggr(i)=j means that node i in the adjacency graph
!                  of the fine-level matrix is mapped onto node j in the
!                  adjacency graph of the coarse-level matrix. Note that the indices
!                  are assumed to be shifted so as to make sure the ranges on
!                  the various processes do not   overlap.
!    nlaggr     -  integer, dimension(:) input
!                  nlaggr(i) contains the aggregates held by process i.
!    op_prol    -  type(psb_cspmat_type), input/output
!                  The tentative prolongator on input, the computed prolongator on output
!
!    op_restr    -  type(psb_cspmat_type), output
!                  The restrictor operator; in this particular case, it is different
!                  from the transpose of the prolongator.
!
!    info       -  integer, output.
!                  Error code.
!
!
subroutine amg_caggrmat_minnrg_bld(a,desc_a,ilaggr,nlaggr,parms,&
     & ac,desc_ac,op_prol,op_restr,t_prol,info)
  use psb_base_mod
  use amg_base_prec_type
  use amg_c_inner_mod, amg_protect_name => amg_caggrmat_minnrg_bld

  implicit none

  ! Arguments
  type(psb_cspmat_type), intent(in)           :: a
  type(psb_desc_type), intent(inout)            :: desc_a
  integer(psb_lpk_), intent(inout)              :: ilaggr(:), nlaggr(:)
  type(amg_sml_parms), intent(inout)         :: parms
  type(psb_lcspmat_type), intent(inout)       :: t_prol
  type(psb_cspmat_type), intent(inout)       :: op_prol, ac,op_restr
  type(psb_desc_type), intent(inout)            :: desc_ac
  integer(psb_ipk_), intent(out)                :: info

  ! Local variables
  integer(psb_lpk_) :: nrow, nglob, ncol, ntaggr, nzac, ip, ndx,&
       & naggr, nzl,naggrm1,naggrp1, i, j, k, jd, icolF, nrt
  type(psb_ctxt_type)       :: ctxt
  integer(psb_ipk_)         :: np, me, icomm
  character(len=20)         :: name
  type(psb_lcspmat_type)      :: la, af, ptilde, rtilde, atran, atp, atdatp
  type(psb_lcspmat_type)      :: am3,am4, ap, adap,atmp,rada, ra, atmp2, dap, dadap, da
  type(psb_lcspmat_type)      :: dat, datp, datdatp, atmp3, tmp_prol
  type(psb_lc_coo_sparse_mat) :: tmpcoo
  type(psb_lc_csr_sparse_mat) :: acsr1, acsr2, acsr3, acsr, acsrf
  type(psb_lc_csc_sparse_mat) :: csc_dap, csc_dadap, csc_datp, csc_datdatp, acsc
  complex(psb_spk_), allocatable :: adiag(:), adinv(:)
  complex(psb_spk_), allocatable :: omf(:), omp(:), omi(:), oden(:)
  logical                    :: filter_mat
  integer(psb_ipk_)          :: ierr(5)
  integer(psb_ipk_)          :: debug_level, debug_unit, err_act
  integer(psb_ipk_), parameter :: ncmax=16
  real(psb_spk_)              :: anorm, theta
  complex(psb_spk_)            :: tmp, alpha, beta, ommx

  name='amg_aggrmat_minnrg'
  info=psb_success_
  call psb_erractionsave(err_act)
  if (psb_errstatus_fatal()) then
    info = psb_err_internal_error_; goto 9999
  end if
  debug_unit  = psb_get_debug_unit()
  debug_level = psb_get_debug_level()

  ctxt = desc_a%get_context()
  icomm = desc_a%get_mpic()

  call psb_info(ctxt, me, np)

  nglob = desc_a%get_global_rows()
  nrow  = desc_a%get_local_rows()
  ncol  = desc_a%get_local_cols()

  theta = parms%aggr_thresh

  naggr  = nlaggr(me+1)
  ntaggr = sum(nlaggr)

  naggrm1 = sum(nlaggr(1:me))
  naggrp1 = sum(nlaggr(1:me+1))

  filter_mat = (parms%aggr_filter == amg_filter_mat_)

  !NEEDS TO BE REWORKED !!

  ! naggr: number of local aggregates
  ! nrow: local rows.
  !
  allocate(adinv(ncol),&
       & omf(ncol),omp(ntaggr),oden(ntaggr),omi(ncol),stat=info)

  if (info /= psb_success_) then
    info=psb_err_alloc_request_; ierr(1)=6*ncol+ntaggr;
    call psb_errpush(info,name,i_err=ierr,a_err='complex(psb_spk_)')
    goto 9999
  end if

!!$  ! Get the diagonal D
!!$  adiag =  a%get_diag(info)
!!$  if (info == psb_success_) &
!!$       & call psb_realloc(ncol,adiag,info)
!!$  if (info == psb_success_) &
!!$       & call psb_halo(adiag,desc_a,info)
!!$  if (info == psb_success_) call a%cp_to_l(la)
!!$  if (info /= psb_success_) then
!!$    call psb_errpush(psb_err_from_subroutine_,name,a_err='sp_getdiag')
!!$    goto 9999
!!$  end if
!!$
!!$  do i=1,size(adiag)
!!$    if (adiag(i) /= czero) then
!!$      adinv(i) = cone / adiag(i)
!!$    else
!!$      adinv(i) = cone
!!$    end if
!!$  end do
!!$
!!$
!!$
!!$  ! 1. Allocate Ptilde in sparse matrix form
!!$  call op_prol%mv_to(tmpcoo)
!!$  call ptilde%mv_from(tmpcoo)
!!$  call ptilde%cscnv(info,type='csr')
!!$
!!$  if (info == psb_success_) call la%cscnv(am3,info,type='csr',dupl=psb_dupl_add_)
!!$  if (info == psb_success_) call la%cscnv(da,info,type='csr',dupl=psb_dupl_add_)
!!$  if (info /= psb_success_) then
!!$    call psb_errpush(psb_err_from_subroutine_,name,a_err='spcnv')
!!$    goto 9999
!!$  end if
!!$  if (debug_level >= psb_debug_outer_) &
!!$       & write(debug_unit,*) me,' ',trim(name),&
!!$       & ' Initial copies done.'
!!$
!!$  call da%scal(adinv,info)
!!$
!!$  call psb_spspmm(da,ptilde,dap,info)
!!$
!!$  if(info /= psb_success_) then
!!$    call psb_errpush(psb_err_from_subroutine_,name,a_err='spspmm 1')
!!$    goto 9999
!!$  end if
!!$
!!$  call dap%clone(atmp,info)
!!$
!!$  call psb_sphalo(atmp,desc_a,am4,info,&
!!$       & colcnv=.false.,rowscale=.true.,outfmt='CSR  ')
!!$  if (info == psb_success_) call psb_rwextd(ncol,atmp,info,b=am4)
!!$  if (info == psb_success_) call am4%free()
!!$
!!$  call psb_spspmm(da,atmp,dadap,info)
!!$  call atmp%free()
!!$
!!$  !  !$  write(0,*) 'Columns of AP',psb_sp_get_ncols(ap)
!!$  !  !$  write(0,*) 'Columns of ADAP',psb_sp_get_ncols(adap)
!!$  call dap%mv_to(csc_dap)
!!$  call dadap%mv_to(csc_dadap)
!!$
!!$  call csc_mat_col_prod(csc_dap,csc_dadap,omp,info)
!!$  call csc_mat_col_prod(csc_dadap,csc_dadap,oden,info)
!!$  call psb_sum(ctxt,omp)
!!$  call psb_sum(ctxt,oden)
!!$  ! !$  write(0,*) trim(name),' OMP :',omp
!!$  ! !$  write(0,*) trim(name),' ODEN:',oden
!!$
!!$  omp = omp/oden
!!$
!!$  ! !$  write(0,*) 'Check on output prolongator ',omp(1:min(size(omp),10))
!!$  if (debug_level >= psb_debug_outer_) &
!!$       & write(debug_unit,*) me,' ',trim(name),&
!!$       & 'Done NUMBMM 1'
!!$
!!$  call am3%mv_to(acsr3)
!!$  ! Compute omega_int
!!$  ommx = czero
!!$  do i=1, ncol
!!$    if (ilaggr(i) >0) then
!!$      omi(i) = omp(ilaggr(i))
!!$    else
!!$      omi(i) = czero
!!$    end if
!!$    if(abs(omi(i)) .gt. abs(ommx)) ommx = omi(i)
!!$  end do
!!$  ! Compute omega_fine
!!$  do i=1, nrow
!!$    omf(i) = ommx
!!$    do j=acsr3%irp(i),acsr3%irp(i+1)-1
!!$      if(abs(omi(acsr3%ja(j))) .lt. abs(omf(i))) omf(i)=omi(acsr3%ja(j))
!!$    end do
!!$ ! !     if(min(real(omf(i)),aimag(omf(i))) < szero) omf(i) = czero
!!$    if(psb_minreal(omf(i)) < szero) omf(i) = czero
!!$  end do
!!$
!!$  omf(1:nrow) = omf(1:nrow) * adinv(1:nrow)
!!$
!!$  if (filter_mat) then
!!$    !
!!$    ! Build the filtered matrix Af from A
!!$    !
!!$    call la%cscnv(acsrf,info,dupl=psb_dupl_add_)
!!$
!!$    do i=1,nrow
!!$      tmp = czero
!!$      jd  = -1
!!$      do j=acsrf%irp(i),acsrf%irp(i+1)-1
!!$        if (acsrf%ja(j) == i) jd = j
!!$        if (abs(acsrf%val(j)) < theta*sqrt(abs(adiag(i)*adiag(acsrf%ja(j))))) then
!!$          tmp=tmp+acsrf%val(j)
!!$          acsrf%val(j)=czero
!!$        endif
!!$      enddo
!!$      if (jd == -1) then
!!$        write(0,*) 'Wrong input: we need the diagonal!!!!', i
!!$      else
!!$        acsrf%val(jd)=acsrf%val(jd)-tmp
!!$      end if
!!$    enddo
!!$    ! Take out zeroed terms
!!$    call acsrf%clean_zeros(info)
!!$
!!$    !
!!$    ! Build the smoothed prolongator using the filtered matrix
!!$    !
!!$    do i=1,acsrf%get_nrows()
!!$      do j=acsrf%irp(i),acsrf%irp(i+1)-1
!!$        if (acsrf%ja(j) == i) then
!!$          acsrf%val(j) = cone - omf(i)*acsrf%val(j)
!!$        else
!!$          acsrf%val(j) = - omf(i)*acsrf%val(j)
!!$        end if
!!$      end do
!!$    end do
!!$
!!$    if (debug_level >= psb_debug_outer_) &
!!$         & write(debug_unit,*) me,' ',trim(name),&
!!$         & 'Done gather, going for SYMBMM 1'
!!$
!!$    call af%mv_from(acsrf)
!!$    !
!!$    ! op_prol = (I-w*D*Af)Ptilde
!!$    ! Doing it this way means to consider diag(Af_i)
!!$    !
!!$    !
!!$    call psb_spspmm(af,ptilde,op_prol,info)
!!$    if (debug_level >= psb_debug_outer_) &
!!$         & write(debug_unit,*) me,' ',trim(name),&
!!$         & 'Done SPSPMM 1'
!!$  else
!!$    !
!!$    ! Build the smoothed prolongator using the original matrix
!!$    !
!!$    do i=1,acsr3%get_nrows()
!!$      do j=acsr3%irp(i),acsr3%irp(i+1)-1
!!$        if (acsr3%ja(j) == i) then
!!$          acsr3%val(j) = cone - omf(i)*acsr3%val(j)
!!$        else
!!$          acsr3%val(j) = - omf(i)*acsr3%val(j)
!!$        end if
!!$      end do
!!$    end do
!!$
!!$    call am3%mv_from(acsr3)
!!$    if (debug_level >= psb_debug_outer_) &
!!$         & write(debug_unit,*) me,' ',trim(name),&
!!$         & 'Done gather, going for SYMBMM 1'
!!$    !
!!$    !
!!$    ! op_prol = (I-w*D*A)Ptilde
!!$    !
!!$    !
!!$    call psb_spspmm(am3,ptilde,op_prol,info)
!!$    if (debug_level >= psb_debug_outer_) &
!!$         & write(debug_unit,*) me,' ',trim(name),&
!!$         & 'Done NUMBMM 1'
!!$
!!$  end if
!!$
!!$
!!$  !
!!$  ! Ok, let's start over with the restrictor
!!$  !
!!$  call ptilde%transc(rtilde)
!!$  call la%cscnv(atmp,info,type='csr')
!!$  call psb_sphalo(atmp,desc_a,am4,info,&
!!$       & colcnv=.true.,rowscale=.true.)
!!$  nrt  = am4%get_nrows()
!!$  call am4%csclip(atmp2,info,lone,nrt,lone,ncol)
!!$  call atmp2%cscnv(info,type='CSR')
!!$  if (info == psb_success_) call psb_rwextd(ncol,atmp,info,b=atmp2)
!!$  call am4%free()
!!$  call atmp2%free()
!!$
!!$  ! This is to compute the transpose. It ONLY works if the
!!$  ! original A has a symmetric pattern.
!!$  call atmp%transc(atmp2)
!!$  call atmp2%csclip(dat,info,lone,nrow,lone,ncol)
!!$  call dat%cscnv(info,type='csr')
!!$  call dat%scal(adinv,info)
!!$
!!$  ! Now for the product.
!!$  call psb_spspmm(dat,ptilde,datp,info)
!!$
!!$  call datp%clone(atmp2,info)
!!$  call psb_sphalo(atmp2,desc_a,am4,info,&
!!$       & colcnv=.false.,rowscale=.true.,outfmt='CSR  ')
!!$  if (info == psb_success_) call psb_rwextd(ncol,atmp2,info,b=am4)
!!$  if (info == psb_success_) call am4%free()
!!$
!!$
!!$  call psb_symbmm(dat,atmp2,datdatp,info)
!!$  call psb_numbmm(dat,atmp2,datdatp)
!!$  call atmp2%free()
!!$
!!$  call datp%mv_to(csc_datp)
!!$  call datdatp%mv_to(csc_datdatp)
!!$
!!$  call csc_mat_col_prod(csc_datp,csc_datdatp,omp,info)
!!$  call csc_mat_col_prod(csc_datdatp,csc_datdatp,oden,info)
!!$  call psb_sum(ctxt,omp)
!!$  call psb_sum(ctxt,oden)
!!$
!!$
!!$  ! !$  write(debug_unit,*) trim(name),' OMP_R :',omp
!!$  ! ! $  write(debug_unit,*) trim(name),' ODEN_R:',oden
!!$  omp = omp/oden
!!$  ! !$  write(0,*) 'Check on output restrictor',omp(1:min(size(omp),10))
!!$  ! Compute omega_int
!!$  ommx = czero
!!$  do i=1, ncol
!!$    if (ilaggr(i) >0) then
!!$      omi(i) = omp(ilaggr(i))
!!$    else
!!$      omi(i) = czero
!!$    end if
!!$    if(abs(omi(i)) .gt. abs(ommx)) ommx = omi(i)
!!$  end do
!!$  ! Compute omega_fine
!!$  ! Going over the columns of atmp means going over the rows
!!$  ! of A^T. Hopefully ;-)
!!$  call atmp%cp_to(acsc)
!!$
!!$  do i=1, nrow
!!$    omf(i) = ommx
!!$    do j= acsc%icp(i),acsc%icp(i+1)-1
!!$      if(abs(omi(acsc%ia(j))) .lt. abs(omf(i))) omf(i)=omi(acsc%ia(j))
!!$    end do
!!$ ! !    if(min(real(omf(i)),aimag(omf(i))) < szero) omf(i) = czero
!!$    if(psb_minreal(omf(i)) < szero) omf(i) = czero
!!$  end do
!!$  omf(1:nrow) = omf(1:nrow)*adinv(1:nrow)
!!$  call psb_halo(omf,desc_a,info)
!!$  call acsc%free()
!!$
!!$
!!$  call atmp%mv_to(acsr1)
!!$
!!$  do i=1,acsr1%get_nrows()
!!$    do j=acsr1%irp(i),acsr1%irp(i+1)-1
!!$      if (acsr1%ja(j) == i) then
!!$        acsr1%val(j) = cone - acsr1%val(j)*omf(acsr1%ja(j))
!!$      else
!!$        acsr1%val(j) =      - acsr1%val(j)*omf(acsr1%ja(j))
!!$      end if
!!$    end do
!!$  end do
!!$  call atmp%mv_from(acsr1)
!!$
!!$  call rtilde%mv_to(tmpcoo)
!!$  nzl = tmpcoo%get_nzeros()
!!$  i=0
!!$  do k=1, nzl
!!$    if ((naggrm1 < tmpcoo%ia(k)) .and. (tmpcoo%ia(k) <= naggrp1)) then
!!$      i = i+1
!!$      tmpcoo%val(i) = tmpcoo%val(k)
!!$      tmpcoo%ia(i)  = tmpcoo%ia(k)
!!$      tmpcoo%ja(i)  = tmpcoo%ja(k)
!!$    end if
!!$  end do
!!$  call tmpcoo%set_nzeros(i)
!!$  call rtilde%mv_from(tmpcoo)
!!$  call rtilde%cscnv(info,type='csr')
!!$
!!$  call psb_spspmm(rtilde,atmp,op_restr,info)
!!$
!!$  !
!!$  ! Now we have to gather the halo of op_prol, and add it to itself
!!$  ! to multiply it by A,
!!$  !
!!$  call op_prol%clone(tmp_prol,info)
!!$  if (info == psb_success_) call psb_sphalo(tmp_prol,desc_a,am4,info,&
!!$       & colcnv=.false.,rowscale=.true.)
!!$  if (info == psb_success_) call psb_rwextd(ncol,tmp_prol,info,b=am4)
!!$  if (info == psb_success_) call am4%free()
!!$
!!$  if(info /= psb_success_) then
!!$    call psb_errpush(psb_err_internal_error_,name,a_err='Halo of op_prol')
!!$    goto 9999
!!$  end if
!!$
!!$  !
!!$  ! Now we have to fix this.  The only rows of B that are correct
!!$  ! are those corresponding to "local" aggregates, i.e. indices in ilaggr(:)
!!$  !
!!$  call op_restr%mv_to(tmpcoo)
!!$
!!$  nzl = tmpcoo%get_nzeros()
!!$  i=0
!!$  do k=1, nzl
!!$    if ((naggrm1 < tmpcoo%ia(k)) .and. (tmpcoo%ia(k) <= naggrp1)) then
!!$      i = i+1
!!$      tmpcoo%val(i) = tmpcoo%val(k)
!!$      tmpcoo%ia(i)  = tmpcoo%ia(k)
!!$      tmpcoo%ja(i)  = tmpcoo%ja(k)
!!$    end if
!!$  end do
!!$  call tmpcoo%set_nzeros(i)
!!$  call op_restr%mv_from(tmpcoo)
!!$  call op_restr%cscnv(info,type='csr')
!!$
!!$
!!$  if (debug_level >= psb_debug_outer_) &
!!$       & write(debug_unit,*) me,' ',trim(name),&
!!$       & 'starting sphalo/ rwxtd'
!!$
!!$  call psb_spspmm(la,tmp_prol,am3,info)
!!$  if (debug_level >= psb_debug_outer_) &
!!$       & write(debug_unit,*) me,' ',trim(name),&
!!$       & 'Done SPSPMM 2'
!!$
!!$  call psb_sphalo(am3,desc_a,am4,info,&
!!$       & colcnv=.false.,rowscale=.true.)
!!$  if (info == psb_success_) call psb_rwextd(ncol,am3,info,b=am4)
!!$  if (info == psb_success_) call am4%free()
!!$
!!$  if(info /= psb_success_) then
!!$    call psb_errpush(psb_err_internal_error_,name,&
!!$         & a_err='Extend am3')
!!$    goto 9999
!!$  end if
!!$  if (debug_level >= psb_debug_outer_) &
!!$       & write(debug_unit,*) me,' ',trim(name),&
!!$       & 'Done sphalo/ rwxtd'
!!$
!!$  call psb_spspmm(op_restr,am3,ac,info)
!!$  if (info == psb_success_) call am3%free()
!!$  if (info == psb_success_) call ac%cscnv(info,type='coo',dupl=psb_dupl_add_)
!!$
!!$  if (info /= psb_success_) then
!!$    call psb_errpush(psb_err_internal_error_,name,&
!!$         &a_err='Build ac = op_restr x am3')
!!$    goto 9999
!!$  end if

  if (debug_level >= psb_debug_outer_) &
       & write(debug_unit,*) me,' ',trim(name),&
       & 'Done smooth_aggregate '
  call psb_erractionrestore(err_act)
  return

9999 continue
  call psb_errpush(info,name)
  call psb_error_handler(err_act)
  return

contains

  subroutine csc_mat_col_prod(a,b,v,info)
    implicit none
    type(psb_lc_csc_sparse_mat), intent(in) :: a, b
    complex(psb_spk_), intent(out)             :: v(:)
    integer(psb_ipk_), intent(out)           :: info

    integer(psb_lpk_)                           :: i,j,k, nr, nc,iap,nra,ibp,nrb

    info = psb_success_
    nc   = a%get_ncols()
    if (nc /= b%get_ncols()) then
      write(0,*) 'Matrices A and B should have same columns'
      info = -1
      return
    end if

    do j=1, nc
      iap  = a%icp(j)
      nra  = a%icp(j+1)-iap
      ibp  = b%icp(j)
      nrb  = b%icp(j+1)-ibp
      v(j) = sparse_srtd_dot(nra,a%ia(iap:iap+nra-1),a%val(iap:iap+nra-1),&
           & nrb,b%ia(ibp:ibp+nrb-1),b%val(ibp:ibp+nrb-1))
    end do

  end subroutine csc_mat_col_prod

  subroutine csr_mat_row_prod(a,b,v,info)
    implicit none
    type(psb_lc_csr_sparse_mat), intent(in) :: a, b
    complex(psb_spk_), intent(out)             :: v(:)
    integer(psb_ipk_), intent(out)           :: info

    integer(psb_lpk_)                        :: i,j,k, nr, nc,iap,nca,ibp,ncb

    info = psb_success_
    nr   = a%get_nrows()
    if (nr /= b%get_nrows()) then
      write(0,*) 'Matrices A and B should have same rows'
      info = -1
      return
    end if

    do j=1, nr
      iap  = a%irp(j)
      nca  = a%irp(j+1)-iap
      ibp  = b%irp(j)
      ncb  = b%irp(j+1)-ibp
      v(j) = sparse_srtd_dot(nca,a%ja(iap:iap+nca-1),a%val(iap:iap+nca-1),&
           & ncb,b%ja(ibp:ibp+ncb-1),b%val(ibp:ibp+ncb-1))
    end do

  end subroutine csr_mat_row_prod

  function sparse_srtd_dot(nv1,iv1,v1,nv2,iv2,v2) result(dot)
    implicit none
    integer(psb_lpk_), intent(in) :: nv1,nv2
    integer(psb_lpk_), intent(in) :: iv1(:), iv2(:)
    complex(psb_spk_), intent(in) :: v1(:),v2(:)
    complex(psb_spk_)      :: dot

    integer(psb_lpk_) :: i,j,k, ip1, ip2

    dot = czero
    ip1 = 1
    ip2 = 1

    do
      if (ip1 > nv1) exit
      if (ip2 > nv2) exit
      if (iv1(ip1) == iv2(ip2)) then
        dot = dot + conjg(v1(ip1))*v2(ip2)
        ip1 = ip1 + 1
        ip2 = ip2 + 1
      else if (iv1(ip1) < iv2(ip2)) then
        ip1 = ip1 + 1
      else
        ip2 = ip2 + 1
      end if
    end do

  end function sparse_srtd_dot

  subroutine local_dump(me,mat,name,header)
    type(psb_lcspmat_type), intent(in) :: mat
    integer(psb_ipk_), intent(in)       :: me
    character(len=*), intent(in)        :: name
    character(len=*), intent(in)        :: header
    character(len=80) :: filename

    write(filename,'(a,a,i0,a,i0,a)') trim(name),'.p',me
    open(20+me,file=filename)
    call mat%print(20+me,head=trim(header))
    close(20+me)
  end subroutine local_dump

end subroutine amg_caggrmat_minnrg_bld


MODULE physical_constants
  IMPLICIT NONE
  DOUBLE PRECISION, PARAMETER :: pi           = 4.0*ATAN(1.0)
  DOUBLE PRECISION, PARAMETER :: e            = 1.602176462E-19
  DOUBLE PRECISION, PARAMETER :: h_P          = 6.62606957E-34
  DOUBLE PRECISION, PARAMETER :: hbar         = 1.054571800E-34
  DOUBLE PRECISION, PARAMETER :: mu_B         = 9.274009994E-24
  DOUBLE PRECISION, PARAMETER :: k_B          = 1.3806488E-23
  DOUBLE PRECISION, PARAMETER :: mu_cero      = 12.566370614E-7
  DOUBLE PRECISION, PARAMETER :: epsilon_cero = 8.854187817E-12
  DOUBLE PRECISION, PARAMETER :: amu          = 1.660538921E-27
  DOUBLE PRECISION, PARAMETER :: g_t          = 9.8
  DOUBLE PRECISION, PARAMETER :: SB_ct        = 5.6704E-8
  COMPLEX*16,       PARAMETER :: J_IMAG       = DCMPLX(0.0,1.0)
  DOUBLE PRECISION, PARAMETER :: speedoflight = 299792458.0
  DOUBLE PRECISION            :: TOTAL_TIME
END MODULE physical_constants

MODULE ARRAYS

  DOUBLE PRECISION, DIMENSION(:,:), ALLOCATABLE :: Identity,j_x,j_y,j_z,I_x,I_y,I_z
  COMPLEX*16,       DIMENSION(:,:), ALLOCATABLE :: HAMILTONIAN
  COMPLEX*16,TARGET,DIMENSION(:,:), ALLOCATABLE :: H_FLOQUET
  COMPLEX*16,       DIMENSION(:,:), ALLOCATABLE :: H_IJ,U_ZEEMAN
  COMPLEX*16,       DIMENSION(:,:), ALLOCATABLE :: H_FLOQUET_COPY

END MODULE ARRAYS

MODULE subinterface
  IMPLICIT NONE
  INTERFACE
     SUBROUTINE I_and_J_representations(j_x,j_y,j_z,I_x,I_y,I_z,L,S,I)

       IMPLICIT  NONE
       DOUBLE PRECISION, DIMENSION(:,:),INTENT(INOUT) :: j_x,j_y,j_z,I_x,I_y,I_z
       DOUBLE PRECISION, INTENT(IN) :: L,S,I
     END SUBROUTINE I_and_J_representations

     SUBROUTINE F_representation(Fx,Fy,Fz,Ftotal_)

       IMPLICIT NONE
       DOUBLE PRECISION, DIMENSION(:,:), INTENT(OUT):: Fx,Fy,Fz
       DOUBLE PRECISION, INTENT(IN) :: Ftotal_
     END SUBROUTINE F_representation

     SUBROUTINE SET_ATOMIC_PARAMETERS(ATOMICSPECIE,MANIFOLD,JTOTAL,ID,INFO)
       ! ATOMICSPECIE: 87Rb,6Li,Cs,41K,qubit,lattice, SPIN
       ! MANIFOLD : "U" UPPER HYPERFINE MANIFOLD, "L" LOWER HYPERFIND MANIFOLD, "B" BOTH
       ! JTOTAL   :  IF ATOMICSPECIE .EQ. SPIN THEN JTOTAL IS THE TOTAL ANGULAR MOMENTUM OF THE SPIN
       !             IF ATOMICSPECIE .EQ. LATTICE, THEN JTOTAL IS THE NUMBER OF SITES
       USE TYPES
       IMPLICIT NONE
       CHARACTER (LEN=*),OPTIONAL, INTENT(IN) :: ATOMICSPECIE
       CHARACTER (LEN=*),OPTIONAL, INTENT(IN) :: MANIFOLD  !
       !INTEGER,          OPTIONAL, INTENT(IN) :: JTOTAL
       DOUBLE PRECISION,          OPTIONAL, INTENT(IN) :: JTOTAL
       TYPE(ATOM),OPTIONAL,INTENT(OUT) :: ID
       INTEGER, INTENT(INOUT) :: INFO
     END SUBROUTINE SET_ATOMIC_PARAMETERS

     SUBROUTINE MICROMOTIONFOURIERDRESSEDBASIS(ID,DRESSINGFIELDS_INDICES,MODES_NUM,FIELDS, U_FD,E_DRESSED,INFO)
       ! ID        (in)    :: TYPE(ATOM) system ID
       ! DRESSINGFIELDS_INDICES (in) :: integer array indicating the indices of the dressing modes
       ! MODES_NUM (in)    :: integer array indicating the number of harmonics of all driving modes
       ! FIELDS    (in)    :: Array of TYPE(MODE) of dimension
       ! U_FD      (out)   :: complex*16 matrix fourier decomposition of the micromotion operator of the dressed basis
       ! E_DRESSED (out)   :: dressed energies
       ! INFO      (inout) :: error flag
       USE TYPES

       TYPE(ATOM),                     INTENT(IN)  :: ID
       INTEGER,    DIMENSION(:),       INTENT(IN)  :: DRESSINGFIELDS_INDICES
       INTEGER,    DIMENSION(:),       INTENT(IN)  :: MODES_NUM
       TYPE(MODE), DIMENSION(:),       INTENT(IN)  :: FIELDS
       COMPLEX*16, DIMENSION(:,:),     INTENT(OUT) :: U_FD
       DOUBLE PRECISION, DIMENSION(:), INTENT(OUT) :: E_DRESSED
     END SUBROUTINE MICROMOTIONFOURIERDRESSEDBASIS

     SUBROUTINE MICROMOTIONDRESSEDBASIS(ID,MODES_NUM,DRESSINGFIELDS_INDICES,&
          & FIELDS,U_F_MODES,E_MULTIFLOQUET,T1,U,INFO)

       ! ID (in)        :: TYPE(ATOM) system ID
       ! MODES_NUM (in) :: integer array indicating the number of harmonics of each driving mode
       ! DRESSINFIELDS_INDICES :: integer array indicating the indices of the dressing modes
       ! FIELDS         :: Array of TYPE(MODES) with NM components (all driving fields)
       ! U_F_MODES      :: complex*16 matrix of dimension DxD. Fourier decomposition of the micromotion operator of the dressed basis
       ! E_MULTIFLOQUET :: dressed energies
       ! T1             :: double precision, time
       ! U              :: complex*16 matrix of dimension D_BARE x D_BARE. micromotion operator at time T1
       ! INFO           :: error flag

       USE TYPES
       IMPLICIT NONE
       TYPE(ATOM),                       INTENT(IN)    :: ID
       INTEGER,          DIMENSION(:),   INTENT(IN)    :: MODES_NUM
       INTEGER,          DIMENSION(:),   INTENT(IN)    :: DRESSINGFIELDS_INDICES
       COMPLEX*16,       DIMENSION(:,:), INTENT(IN)    :: U_F_MODES
       DOUBLE PRECISION, DIMENSION(:),   INTENT(IN)    :: E_MULTIFLOQUET
       TYPE(MODE),       DIMENSION(:),   INTENT(IN)    :: FIELDS
       DOUBLE PRECISION ,                INTENT(IN)    :: T1
       COMPLEX*16,       DIMENSION(:,:), INTENT(OUT)   :: U
       INTEGER,                          INTENT(INOUT) :: INFO
     END SUBROUTINE MICROMOTIONDRESSEDBASIS
  END INTERFACE
END MODULE subinterface

MODULE SUBINTERFACE_LAPACK

  IMPLICIT NONE
  PUBLIC
  INTERFACE

     SUBROUTINE WRITE_MATRIX(A)
       DOUBLE PRECISION, DIMENSION(:,:) :: A
     END SUBROUTINE  WRITE_MATRIX

     SUBROUTINE WRITE_MATRIX_INT(A)
       INTEGER, DIMENSION(:,:) :: A
     END SUBROUTINE  WRITE_MATRIX_INT

  END INTERFACE
END MODULE SUBINTERFACE_LAPACK

MODULE QUICKSORTINTERFACE
  IMPLICIT NONE
  PUBLIC
  INTERFACE
     recursive subroutine QUICK_SORT_I_T(a,idx_a,na)
       implicit none

       ! DUMMY ARGUMENTS
       integer,                         intent(in)    :: na       ! nr or items to sort
       DOUBLE PRECISION, dimension(nA), intent(inout) :: a     ! vector to be sorted
       integer,          dimension(nA), intent(inout) :: idx_a ! sorted indecies of a
     end subroutine QUICK_SORT_I_T

!     recursive subroutine QUICK_SORT_INTEGERS(a,idx_a,na)
!     implicit none

     ! DUMMY ARGUMENTS
!     integer,                intent(in) :: na       ! nr or items to sort
!     integer, dimension(nA), intent(inout) :: a     ! vector to be sorted
!     integer, dimension(nA), intent(inout) :: idx_a ! sorted indecies of a

!     end subroutine quick_sort_integers

     subroutine InsertionSort_I(a,idx_a,na)

     !use sangoma_base, only: REALPREC, INTPREC

     implicit none

     ! DUMMY ARGUMENTS
     integer,                         intent(in)    :: na
     integer, dimension(nA), intent(inout) :: a
     integer,          dimension(nA), intent(inout) :: idx_a
     end subroutine InsertionSort_I

     subroutine InsertionSort_d(a,idx_a,na)

     !use sangoma_base, only: REALPREC, INTPREC

     implicit none

     ! DUMMY ARGUMENTS
     integer,                         intent(in)    :: na
     double precision, dimension(nA), intent(inout) :: a
     integer,          dimension(nA), intent(inout) :: idx_a

     end subroutine InsertionSort_d

  END INTERFACE
END MODULE QUICKSORTINTERFACE


c     # ------------------------------------------------------------
c     # Prescribes velocity fields for the unit sphere.
c     #
c     # Assumes all components are given in coordinates relative to
c     # the standard basis (1,0) and (0,1).
c     # ------------------------------------------------------------

      subroutine velocity_components_spherical(x,y,t, u,
     &                                              vcart,flag)
      implicit none

      double precision x, y, t, u(2),vcart(3)
      double precision derivs(4)
      integer flag

      call velocity_derivs(x,y,t, u,vcart, derivs, flag)

      end

      subroutine velocity_derivs(x,y,t, u, vcart, derivs, flag)
      implicit none

      double precision x, y, t, u(2)
      double precision derivs(4)
      integer flag

      double precision pi, pi2
      common /compi/ pi, pi2

      integer example
      common /example_comm/ example

      double precision omega(3)
      common /rotation_comm/ omega

      double precision u1x, u1y, u2x, u2y
      double precision phi, theta
      double precision kappa, period

      double precision tp, lp
      double precision t1(3), t2(3), xp, yp, zp, w
      double precision rv(3), vcart(3), th, t1n2, t2n2
      double precision map_dot, thetax, thetay, phix, phiy

      double precision lpx, lpy, thx, thy
      double precision cu1, fu1, fu1x, fu1y, gu1, gu1x, gu1y
      double precision cu2, fu2, fu2x, fu2y, gu2, gu2x, gu2y
      double precision hu1, hu1x, hu1y
      double precision gh, ghx, ghy
      double precision zf, zfx, zfy
      double precision uderivs_comp(4), ucomp(2)

c     # uderivs(1) = u1x
c     # uderivs(2) = u1y
c     # uderivs(3) = u2x
c     # uderivs(4) = u2y

      call map_comp2spherical_derivs(x,y,theta,phi,
     &             thetax, thetay, phix, phiy)

      derivs(1) = 0
      derivs(2) = 0
      derivs(3) = 0
      derivs(4) = 0

      flag = 0

      if (example .eq. 0) then
          flag = 1
          call mapc2m_spherical(x,y,xp,yp,zp)
          rv(1) = xp
          rv(2) = yp
          rv(3) = zp
          call map_cross(omega,rv, vcart,w)

c         # Define (du(1)/dx, du(2)/dy, du(3)/dz)
          derivs(1) = 0
          derivs(2) = 0
          derivs(3) = 0
      elseif (example .ge. 1 .and. example .le. 4) then
c         # Divergence-free
          flag = 0

c         # Lauritzen : (lambda,th)  <----> (theta,phi) (here)

          period = 5

          tp = t/period
          lp = theta
          th = phi

          lpx = thetax
          lpy = thetay

          thx = phix
          thy = phiy

          if (example .eq. 1) then

c             # --------------------------------------------------
c             # From Nair and Lauritzen 2010 (Case 1; kappa = 2)
c             # u = kappa*sin(lp)**2*sin(2*th)*cos(pi*t/Tfinal) +
c             #           2*pi*cos(th)/Tfinal
c             # v = kappa*sin(2*lp)*cos(th)*cos(pi*t/Tfinal)
c             # --------------------------------------------------

              kappa = 2.4
              lp = theta   !! Don't shift

c             # u = U(1) component
              cu1 = kappa*cos(pi*tp)

              fu1 = sin(lp/2)**2
              fu1x = 2*sin(lp/2)*cos(lp/2)*lpx/2
              fu1y = 2*sin(lp/2)*cos(lp/2)*lpy/2

              gu1 = sin(2*th)
              gu1x = 2*cos(2*th)*thx
              gu1y = 2*cos(2*th)*thy

              ucomp(1) = cu1*fu1*gu1
              u1x = cu1*(fu1*gu1x + fu1x*gu1)
              u1y = cu1*(fu1*gu1y + fu1y*gu1)

c             # v = U(2) component
              cu2 = kappa/2*cos(pi*tp)

              fu2 = sin(lp)
              fu2x = cos(lp)*lpx
              fu2y = cos(lp)*lpy

              gu2 = cos(th)
              gu2x = -thx*sin(th)
              gu2y = -thy*sin(th)

              ucomp(2) = cu2*fu2*gu2

              u2x = cu2*(fu2*gu2x + fu2x*gu2)
              u2y = cu2*(fu2*gu2y + fu2y*gu2)

          elseif (example .eq. 2 .or. example .eq. 4) then

c             # --------------------------------------------------
c             # From Nair and Lauritzen 2010 (Case 4; kappa = 2)
c             # u = kappa*sin(lp)**2*sin(2*th)*cos(pi*t/Tfinal) +
c             #           2*pi*cos(th)/Tfinal
c             # v = kappa*sin(2*lp)*cos(th)*cos(pi*t/Tfinal)
c             # --------------------------------------------------

              kappa = 2
              if (example .eq. 4) then
                  lp = theta - pi2*tp

c                 # Zonal flow
                  zf = (pi2/period)*cos(th)
                  zfx = -(pi2/period)*sin(th)*thx
                  zfy = 0
              else
                  zf = 0
                  zfx = 0
                  zfy = 0
              endif

c             # u = U(1) component
              cu1 = kappa*cos(pi*tp)

              fu1 = sin(lp)**2
              fu1x = 2*sin(lp)*cos(lp)*lpx
              fu1y = 2*sin(lp)*cos(lp)*lpy

              gu1 = sin(2*th)
              gu1x = 2*cos(2*th)*thx
              gu1y = 2*cos(2*th)*thy

              ucomp(1) = cu1*fu1*gu1 + zf
              u1x = cu1*(fu1*gu1x + fu1x*gu1) + zfx
              u1y = cu1*(fu1*gu1y + fu1y*gu1) + zfy

c             # v = U(2) component
              cu2 = kappa*cos(pi*tp)

              fu2 = sin(2*lp)
              fu2x = 2*cos(2*lp)*lpx
              fu2y = 2*cos(2*lp)*lpy

              gu2 = cos(th)
              gu2x = -thx*sin(th)
              gu2y = -thy*sin(th)

              ucomp(2) = cu2*fu2*gu2

              u2x = cu2*(fu2*gu2x + fu2x*gu2)
              u2y = cu2*(fu2*gu2y + fu2y*gu2)

          elseif (example .eq. 3) then
c             # -----------------------------------------------------
c             # Nair and Lauritzen (2010) Case 3 (kappa = 2)
c             # with background zonal flow
c             #
c             u = -(kappa/2.d0)*sin(lp/2.d0)**2*sin(2*th)*cos(th)**2
c                    *cos(pi*t/Tfinal) + 2*pi*cos(th)/Tfinal
c             v = (kappa/4.d0)*sin(lp)*(cos(th)**3)*cos(pi*t/Tfinal)
c             # -----------------------------------------------------

c             # U(1) component
              kappa = 2
              cu1 = -5.d0/period*cos(pi*tp)

              fu1 = sin(lp/2.d0)**2
              fu1x = 2.d0*sin(lp/2)*cos(lp/2)*lpx/2.d0
              fu1y = 2.d0*sin(lp/2)*cos(lp/2)*lpy/2.d0

              gu1 = sin(2*th)
              gu1x = 2.d0*cos(2*th)*thx
              gu1y = 2.d0*cos(2*th)*thy

              hu1  = cos(th)**2
              hu1x = -2.d0*cos(th)*sin(th)*thx
              hu1y = -2.d0*cos(th)*sin(th)*thy

              gh = gu1*hu1
              ghx = gu1*hu1x + gu1x*hu1
              ghy = gu1*hu1y + gu1y*hu1

              ucomp(1) = cu1*fu1*gu1*hu1

              u1x = cu1*(fu1*ghx + fu1x*gh)
              u1y = cu1*(fu1*ghy + fu1y*gh)

c             # U(2) component
              cu2 = 5.d0/(2.d0*period)*cos(pi*tp)

              fu2 = sin(lp)
              fu2x = cos(lp)*lpx
              fu2y = cos(lp)*lpy

              gu2 = cos(th)**3
              gu2x = -3.d0*cos(th)**2*sin(th)*thx
              gu2y = -3.d0*cos(th)**2*sin(th)*thy

              ucomp(2) = cu2*fu2*gu2

              u2x = cu2*(fu2*gu2x + fu2x*gu2)
              u2y = cu2*(fu2*gu2y + fu2y*gu2)

          endif
c         # ------------------------------
c         # Store derivatives
c         # ------------------------------

          uderivs_comp(1) = u1x
          uderivs_comp(2) = u1y
          uderivs_comp(3) = u2x
          uderivs_comp(4) = u2y

c         # compute d(u1/norm(t1))/dx
c         # compute d(u1/norm(t1))/dy
c         # compute d(u2/norm(t2))/dx
c         # compute d(u2/norm(t2))/dy
          call  map_diff_normalized(x,y,ucomp, uderivs_comp,
     &                                  derivs)

c         # "Normalized" flow field
          call map_covariant_basis(x, y, t1,t2)
          t1n2 = map_dot(t1,t1)
          t2n2 = map_dot(t2,t2)
          u(1) = ucomp(1)/sqrt(t1n2)
          u(2) = ucomp(2)/sqrt(t2n2)
      else
         write(6,'(A,A)') 'sphere_velocity : ',
     &              'No valid example provided'
         stop
      endif

      end

      subroutine velocity_components_cart(x,y,t,vcart)
      implicit none

      double precision x,y,t, vcart(3)
      double precision u(2), t1(3), t2(3)
      integer flag, k

      call velocity_components_spherical(x,y,t, u,vcart,flag)

      if (flag .eq. 0) then
c         # Velocity components are given in spherical components
c         # and must be converted to Cartesian
          call map_covariant_basis(x, y, t1,t2)

          do k = 1,3
              vcart(k) = u(1)*t1(k) + u(2)*t2(k)
          enddo
      endif

      end

c     # ------------------------------------------------------------
c     # Called from qexact
c     # ------------------------------------------------------------
      subroutine velocity_components(x,y,t,u)
      implicit none

      double precision x,y,t, u(2)
      double precision vcart(3), t1(3), t2(3)
      double precision t1n2, t2n2, map_dot
      integer flag

      call velocity_components_spherical(x,y,t, u,vcart,flag)

      if (flag .eq. 1) then
c         # Velocity components are given in Cartesian components
          call map_covariant_basis(x, y, t1,t2)
          t1n2 = map_dot(t1,t1)
          t2n2 = map_dot(t2,t2)
          u(1) = map_dot(vcart,t1)/t1n2
          u(2) = map_dot(vcart,t2)/t2n2
      endif

      end

c     # ----------------------------------------------
c     # Compute divergence of velocity field.
c     # ----------------------------------------------
      double precision function map_divergence(x,y, t)
      implicit none

      double precision x,y, t

      double precision u(2), vcart(3), derivs(4)
      double precision D11, D22, g(2,2,2)
      integer flag

c     # Get g(i,j,k), g = \Gamma(i,j,k)
      call velocity_derivs(x,y,t, u,vcart,derivs,flag)

      if (flag .eq. 0) then
c         # Velocity and derivatives are given in
c         # spherical components
          call map_christoffel_sym(x,y,g)

          D11 = derivs(1) + u(1)*g(1,1,1) + u(2)*g(1,2,1)
          D22 = derivs(4) + u(1)*g(2,1,2) + u(2)*g(2,2,2)

          map_divergence = D11 + D22
      else
c         # Velocity and derivatives are given in
c         # Cartesian components
          map_divergence = derivs(1) + derivs(2) + derivs(3)
      endif

      end

c     # ------------------------------------------------------------
c     # Public interface (called from setaux)
c     # ------------------------------------------------------------
      subroutine center_velocity(x,y,t,vcart)
      implicit none

      double precision x,y,t, vcart(3)

      call velocity_components_cart(x,y,t,vcart)

      end

      subroutine user_velocity_components_cart(x,y,t,vcart)
      implicit none

      double precision x,y,t, vcart(3)

      call velocity_components_cart(x,y,t,vcart)

      end