C$Procedure      M2Q ( Matrix to quaternion )

      SUBROUTINE M2Q ( R, Q )

C$ Abstract
C
C     Find a unit quaternion corresponding to a specified rotation
C     matrix.
C
C$ Disclaimer
C
C     THIS SOFTWARE AND ANY RELATED MATERIALS WERE CREATED BY THE
C     CALIFORNIA INSTITUTE OF TECHNOLOGY (CALTECH) UNDER A U.S.
C     GOVERNMENT CONTRACT WITH THE NATIONAL AERONAUTICS AND SPACE
C     ADMINISTRATION (NASA). THE SOFTWARE IS TECHNOLOGY AND SOFTWARE
C     PUBLICLY AVAILABLE UNDER U.S. EXPORT LAWS AND IS PROVIDED "AS-IS"
C     TO THE RECIPIENT WITHOUT WARRANTY OF ANY KIND, INCLUDING ANY
C     WARRANTIES OF PERFORMANCE OR MERCHANTABILITY OR FITNESS FOR A
C     PARTICULAR USE OR PURPOSE (AS SET FORTH IN UNITED STATES UCC
C     SECTIONS 2312-2313) OR FOR ANY PURPOSE WHATSOEVER, FOR THE
C     SOFTWARE AND RELATED MATERIALS, HOWEVER USED.
C
C     IN NO EVENT SHALL CALTECH, ITS JET PROPULSION LABORATORY, OR NASA
C     BE LIABLE FOR ANY DAMAGES AND/OR COSTS, INCLUDING, BUT NOT
C     LIMITED TO, INCIDENTAL OR CONSEQUENTIAL DAMAGES OF ANY KIND,
C     INCLUDING ECONOMIC DAMAGE OR INJURY TO PROPERTY AND LOST PROFITS,
C     REGARDLESS OF WHETHER CALTECH, JPL, OR NASA BE ADVISED, HAVE
C     REASON TO KNOW, OR, IN FACT, SHALL KNOW OF THE POSSIBILITY.
C
C     RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF
C     THE SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY
C     CALTECH AND NASA FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE
C     ACTIONS OF RECIPIENT IN THE USE OF THE SOFTWARE.
C
C$ Required_Reading
C
C     ROTATION
C
C$ Keywords
C
C     MATH
C     MATRIX
C     ROTATION
C
C$ Declarations

      DOUBLE PRECISION      R ( 3,  3 )
      DOUBLE PRECISION      Q ( 0 : 3 )

C$ Brief_I/O
C
C     Variable  I/O  Description
C     --------  ---  --------------------------------------------------
C     R          I   A rotation matrix.
C     Q          O   A unit quaternion representing R.
C
C$ Detailed_Input
C
C     R              is a rotation matrix.
C
C$ Detailed_Output
C
C     Q              is a unit-length SPICE-style quaternion
C                    representing R. See the discussion of quaternion
C                    styles in Particulars below.
C
C                    Q is a 4-dimensional vector. If R rotates vectors
C                    in the counterclockwise sense by an angle of theta
C                    radians about a unit vector A, where
C
C                       0 < theta < pi
C                         -       -
C
C                    then letting h = theta/2,
C
C                       Q = ( cos(h), sin(h)A ,  sin(h)A ,  sin(h)A ).
C                                            1          2          3
C
C                    The restriction that theta must be in the range
C                    [0, pi] determines the output quaternion Q
C                    uniquely except when theta = pi; in this special
C                    case, both of the quaternions
C
C                       Q = ( 0,  A ,  A ,  A  )
C                                  1    2    3
C                    and
C
C                       Q = ( 0, -A , -A , -A  )
C                                  1    2    3
C
C                   are possible outputs.
C
C$ Parameters
C
C     None.
C
C$ Exceptions
C
C     1)   If R is not a rotation matrix, the error SPICE(NOTAROTATION)
C          is signaled.
C
C$ Files
C
C     None.
C
C$ Particulars
C
C     A unit quaternion is a 4-dimensional vector for which the sum of
C     the squares of the components is 1. Unit quaternions can be used
C     to represent rotations in the following way: given a rotation
C     angle theta, where
C
C        0 < theta < pi
C          -       -
C
C     and a unit vector A, we can represent the transformation that
C     rotates vectors in the counterclockwise sense by theta radians
C     about A using the quaternion Q, where
C
C        Q =
C
C        ( cos(theta/2), sin(theta/2)a , sin(theta/2)a , sin(theta/2)a )
C                                     1               2               3
C
C     As mentioned in Detailed Output, our restriction on the range of
C     theta determines Q uniquely, except when theta = pi.
C
C     The SPICELIB routine Q2M is an one-sided inverse of this routine:
C     given any rotation matrix R, the calls
C
C        CALL M2Q ( R, Q )
C        CALL Q2M ( Q, R )
C
C     leave R unchanged, except for round-off error.  However, the
C     calls
C
C        CALL Q2M ( Q, R )
C        CALL M2Q ( R, Q )
C
C     might preserve Q or convert Q to -Q.
C
C
C
C     Quaternion Styles
C     -----------------
C
C     There are different "styles" of quaternions used in
C     science and engineering applications. Quaternion styles
C     are characterized by
C
C        - The order of quaternion elements
C
C        - The quaternion multiplication formula
C
C        - The convention for associating quaternions
C          with rotation matrices
C
C     Two of the commonly used styles are
C
C        - "SPICE"
C
C           > Invented by Sir William Rowan Hamilton
C           > Frequently used in mathematics and physics textbooks
C
C        - "Engineering"
C
C           > Widely used in aerospace engineering applications
C
C
C     SPICELIB subroutine interfaces ALWAYS use SPICE quaternions.
C     Quaternions of any other style must be converted to SPICE
C     quaternions before they are passed to SPICELIB routines.
C
C
C     Relationship between SPICE and Engineering Quaternions
C     ------------------------------------------------------
C
C     Let M be a rotation matrix such that for any vector V,
C
C        M*V
C
C     is the result of rotating V by theta radians in the
C     counterclockwise direction about unit rotation axis vector A.
C     Then the SPICE quaternions representing M are
C
C        (+/-) (  cos(theta/2),
C                 sin(theta/2) A(1),
C                 sin(theta/2) A(2),
C                 sin(theta/2) A(3)  )
C
C     while the engineering quaternions representing M are
C
C        (+/-) ( -sin(theta/2) A(1),
C                -sin(theta/2) A(2),
C                -sin(theta/2) A(3),
C                 cos(theta/2)       )
C
C     For both styles of quaternions, if a quaternion q represents
C     a rotation matrix M, then -q represents M as well.
C
C     Given an engineering quaternion
C
C        QENG   = ( q0,  q1,  q2,  q3 )
C
C     the equivalent SPICE quaternion is
C
C        QSPICE = ( q3, -q0, -q1, -q2 )
C
C
C     Associating SPICE Quaternions with Rotation Matrices
C     ----------------------------------------------------
C
C     Let FROM and TO be two right-handed reference frames, for
C     example, an inertial frame and a spacecraft-fixed frame. Let the
C     symbols
C
C        V    ,   V
C         FROM     TO
C
C     denote, respectively, an arbitrary vector expressed relative to
C     the FROM and TO frames. Let M denote the transformation matrix
C     that transforms vectors from frame FROM to frame TO; then
C
C        V   =  M * V
C         TO         FROM
C
C     where the expression on the right hand side represents left
C     multiplication of the vector by the matrix.
C
C     Then if the unit-length SPICE quaternion q represents M, where
C
C        q = (q0, q1, q2, q3)
C
C     the elements of M are derived from the elements of q as follows:
C
C          +-                                                         -+
C          |           2    2                                          |
C          | 1 - 2*( q2 + q3 )   2*(q1*q2 - q0*q3)   2*(q1*q3 + q0*q2) |
C          |                                                           |
C          |                                                           |
C          |                               2    2                      |
C      M = | 2*(q1*q2 + q0*q3)   1 - 2*( q1 + q3 )   2*(q2*q3 - q0*q1) |
C          |                                                           |
C          |                                                           |
C          |                                                   2    2  |
C          | 2*(q1*q3 - q0*q2)   2*(q2*q3 + q0*q1)   1 - 2*( q1 + q2 ) |
C          |                                                           |
C          +-                                                         -+
C
C     Note that substituting the elements of -q for those of q in the
C     right hand side leaves each element of M unchanged; this shows
C     that if a quaternion q represents a matrix M, then so does the
C     quaternion -q.
C
C     To map the rotation matrix M to a unit quaternion, we start by
C     decomposing the rotation matrix as a sum of symmetric
C     and skew-symmetric parts:
C
C                                        2
C        M = [ I  +  (1-cos(theta)) OMEGA  ] + [ sin(theta) OMEGA ]
C
C                     symmetric                   skew-symmetric
C
C
C     OMEGA is a skew-symmetric matrix of the form
C
C                   +-             -+
C                   |  0   -n3   n2 |
C                   |               |
C         OMEGA  =  |  n3   0   -n1 |
C                   |               |
C                   | -n2   n1   0  |
C                   +-             -+
C
C     The vector N of matrix entries (n1, n2, n3) is the rotation axis
C     of M and theta is M's rotation angle.  Note that N and theta
C     are not unique.
C
C     Let
C
C        C = cos(theta/2)
C        S = sin(theta/2)
C
C     Then the unit quaternions Q corresponding to M are
C
C        Q = +/- ( C, S*n1, S*n2, S*n3 )
C
C     The mappings between quaternions and the corresponding rotations
C     are carried out by the SPICELIB routines
C
C        Q2M {quaternion to matrix}
C        M2Q {matrix to quaternion}
C
C     M2Q always returns a quaternion with scalar part greater than
C     or equal to zero.
C
C
C     SPICE Quaternion Multiplication Formula
C     ---------------------------------------
C
C     Given a SPICE quaternion
C
C        Q = ( q0, q1, q2, q3 )
C
C     corresponding to rotation axis A and angle theta as above, we can
C     represent Q using "scalar + vector" notation as follows:
C
C        s =   q0           = cos(theta/2)
C
C        v = ( q1, q2, q3 ) = sin(theta/2) * A
C
C        Q = s + v
C
C     Let Q1 and Q2 be SPICE quaternions with respective scalar
C     and vector parts s1, s2 and v1, v2:
C
C        Q1 = s1 + v1
C        Q2 = s2 + v2
C
C     We represent the dot product of v1 and v2 by
C
C        <v1, v2>
C
C     and the cross product of v1 and v2 by
C
C        v1 x v2
C
C     Then the SPICE quaternion product is
C
C        Q1*Q2 = s1*s2 - <v1,v2>  + s1*v2 + s2*v1 + (v1 x v2)
C
C     If Q1 and Q2 represent the rotation matrices M1 and M2
C     respectively, then the quaternion product
C
C        Q1*Q2
C
C     represents the matrix product
C
C        M1*M2
C
C
C$ Examples
C
C     1)  A case amenable to checking by hand calculation:
C
C            To convert the rotation matrix
C
C                     +-              -+
C                     |  0     1    0  |
C                     |                |
C               R  =  | -1     0    0  |
C                     |                |
C                     |  0     0    1  |
C                     +-              -+
C
C            also represented as
C
C               [ pi/2 ]
C                       3
C
C            to a quaternion, we can use the code fragment
C
C               CALL ROTATE (  HALFPI(),  3,  R  )
C               CALL M2Q    (  R,             Q  )
C
C            M2Q will return Q as
C
C               ( sqrt(2)/2, 0, 0, -sqrt(2)/2 )
C
C            Why?  Well, R is a reference frame transformation that
C            rotates vectors by -pi/2 radians about the axis vector
C
C               A  = ( 0, 0, 1 )
C
C            Equivalently, R rotates vectors by pi/2 radians in
C            the counterclockwise sense about the axis vector
C
C               -A = ( 0, 0, -1 )
C
C            so our definition of Q,
C
C               h = theta/2
C
C               Q = ( cos(h), sin(h)A , sin(h)A , sin(h)A  )
C                                    1         2         3
C
C            implies that in this case,
C
C               Q =  ( cos(pi/4),  0,  0, -sin(pi/4)  )
C
C                 =  ( sqrt(2)/2,  0,  0, -sqrt(2)/2  )
C
C
C     2)  Finding a quaternion that represents a rotation specified by
C         a set of Euler angles:
C
C            Suppose our original rotation R is the product
C
C               [ TAU ]  [ pi/2 - DELTA ]  [ ALPHA ]
C                      3                 2          3
C
C            The code fragment
C
C               CALL EUL2M  ( TAU,   HALFPI() - DELTA,   ALPHA,
C              .              3,     2,                  3,      R )
C
C               CALL M2Q    ( R, Q )
C
C            yields a quaternion Q that represents R.
C
C$ Restrictions
C
C     None.
C
C$ Literature_References
C
C     None.
C
C$ Author_and_Institution
C
C     N.J. Bachman   (JPL)
C     W.L. Taber     (JPL)
C
C$ Version
C
C-    SPICELIB Version 2.0.1, 27-FEB-2008 (NJB)
C
C        Updated header; added information about SPICE
C        quaternion conventions. Made various minor edits
C        throughout header.
C
C-    SPICELIB Version 2.0.0, 17-SEP-1999 (WLT)
C
C        The routine was re-implemented to sharpen the numerical
C        stability of the routine and eliminate calls to SIN
C        and COS functions.
C
C-    SPICELIB Version 1.0.1, 10-MAR-1992 (WLT)
C
C        Comment section for permuted index source lines was added
C        following the header.
C
C-    SPICELIB Version 1.0.0, 30-AUG-1990 (NJB)
C
C-&

C$ Index_Entries
C
C     matrix to quaternion
C
C-&

C
C     SPICELIB functions
C
      LOGICAL               ISROT

C
C     Local parameters
C

C
C     NTOL and DETOL are used to determine whether R is a rotation
C     matrix.
C
C     NTOL is the tolerance for the norms of the columns of R.
C
C     DTOL is the tolerance for the determinant of a matrix whose
C     columns are the unitized columns of R.
C
C
      DOUBLE PRECISION      NTOL
      PARAMETER           ( NTOL = 0.1D0 )

      DOUBLE PRECISION      DTOL
      PARAMETER           ( DTOL = 0.1D0 )

C
C     Local Variables
C
      DOUBLE PRECISION      C
      DOUBLE PRECISION      CC4
      DOUBLE PRECISION      FACTOR
      DOUBLE PRECISION      L2
      DOUBLE PRECISION      MTRACE
      DOUBLE PRECISION      POLISH
      DOUBLE PRECISION      S(3)
      DOUBLE PRECISION      S114
      DOUBLE PRECISION      S224
      DOUBLE PRECISION      S334
      DOUBLE PRECISION      TRACE

C
C     If R is not a rotation matrix, we can't proceed.
C
      IF (  .NOT. ISROT ( R, NTOL, DTOL )  ) THEN
         CALL CHKIN  ( 'M2Q' )
         CALL SETMSG ( 'Input matrix was not a rotation.' )
         CALL SIGERR ( 'SPICE(NOTAROTATION)'              )
         CALL CHKOUT ( 'M2Q'                              )
         RETURN

      END IF
C
C
C     If our quaternion is C, S1, S2, S3 (the S's being the imaginary
C     part) and we let
C
C        CSi = C  * Si
C        Sij = Si * Sj
C
C     then the rotation matrix corresponding to our quaternion is:
C
C        R(1,1)      = 1.0D0 - 2*S22 - 2*S33
C        R(2,1)      =         2*S12 + 2*CS3
C        R(3,1)      =         2*S13 - 2*CS2
C
C        R(1,2)      =         2*S12 - 2*CS3
C        R(2,2)      = 1.0D0 - 2*S11 - 2*S33
C        R(3,2)      =         2*S23 + 2*CS1
C
C        R(1,3)      =         2*S13 + 2*CS2
C        R(2,3)      =         2*S23 - 2*CS1
C        R(3,3)      = 1.0D0 - 2*S11 - 2*S22
C
C        From the above we can see that
C
C           TRACE = 3 - 4*(S11 + S22 + S33)
C
C        so that
C
C
C           1.0D0 + TRACE = 4 - 4*(S11 + S22 + S33)
C                         = 4*(CC + S11 + S22 + S33)
C                         - 4*(S11 + S22 + S33)
C                         = 4*CC
C
C        Thus up to sign
C
C          C = 0.5D0 * DSQRT( 1.0D0 + TRACE )
C
C        But we also have
C
C          1.0D0 + TRACE - 2.0D0*R(i,i) = 4.0D0 - 4.0D0(Sii + Sjj + Skk)
C                                       - 2.0D0 + 4.0D0(Sjj + Skk )
C
C                                       = 2.0D0 - 4.0D0*Sii
C
C        So that
C
C           1.0D0 - TRACE + 2.0D0*R(i,i) = 4.0D0*Sii
C
C        and so up to sign
C
C           Si = 0.5D0*DSQRT( 1.0D0 - TRACE + 2.0D0*R(i,i) )
C
C        in addition to this observation, we note that all of the
C        product pairs can easily be computed
C
C         CS1 = (R(3,2) - R(2,3))/4.0D0
C         CS2 = (R(1,3) - R(3,1))/4.0D0
C         CS3 = (R(2,1) - R(1,2))/4.0D0
C         S12 = (R(2,1) + R(1,2))/4.0D0
C         S13 = (R(3,1) + R(1,3))/4.0D0
C         S23 = (R(2,3) + R(3,2))/4.0D0
C
C     But taking sums or differences of numbers that are nearly equal
C     or nearly opposite results in a loss of precision. As a result
C     we should take some care in which terms to select when computing
C     C, S1, S2, S3.  However, by simply starting with one of the
C     large quantities cc, S11, S22, or S33 we can make sure that we
C     use the best of the 6 quantities above when computing the
C     remaining components of the quaternion.
C

      TRACE  = R(1,1) + R(2,2) + R(3,3)
      MTRACE = 1.0D0 - TRACE

      CC4    = 1.0D0  + TRACE
      S114   = MTRACE + 2.0D0*R(1,1)
      S224   = MTRACE + 2.0D0*R(2,2)
      S334   = MTRACE + 2.0D0*R(3,3)

C
C     Note that if you simply add CC4 + S114 + S224 + S334
C     you get four. Thus at least one of the 4 terms is greater than 1.
C
      IF ( 1.0D0 .LE. CC4 ) THEN
         C      =  DSQRT  ( CC4 * 0.25D0 )
         FACTOR =  1.0D0 /( C   * 4.0D0  )

         S(1)   = ( R(3,2) - R(2,3) )*FACTOR
         S(2)   = ( R(1,3) - R(3,1) )*FACTOR
         S(3)   = ( R(2,1) - R(1,2) )*FACTOR

      ELSE IF ( 1.0D0 .LE. S114 ) THEN

         S(1)   = DSQRT  ( S114 * 0.25D0 )
         FACTOR = 1.0D0 /( S(1) * 4.0D0  )

         C      = ( R(3,2) - R(2,3) ) * FACTOR
         S(2)   = ( R(1,2) + R(2,1) ) * FACTOR
         S(3)   = ( R(1,3) + R(3,1) ) * FACTOR

      ELSE IF ( 1.0D0 .LE. S224 ) THEN

         S(2)   = DSQRT  ( S224 * 0.25D0 )
         FACTOR = 1.0D0 /( S(2) * 4.0D0  )

         C      = ( R(1,3) - R(3,1) ) * FACTOR
         S(1)   = ( R(1,2) + R(2,1) ) * FACTOR
         S(3)   = ( R(2,3) + R(3,2) ) * FACTOR

      ELSE

         S(3)   = DSQRT  ( S334 * 0.25D0 )
         FACTOR = 1.0D0 /( S(3) * 4.0D0  )

         C      = ( R(2,1) - R(1,2) ) * FACTOR
         S(1)   = ( R(1,3) + R(3,1) ) * FACTOR
         S(2)   = ( R(2,3) + R(3,2) ) * FACTOR

      END IF
C
C     If the magnitude of this quaternion is not one, we polish it
C     up a bit.
C
      L2 = C*C + S(1)*S(1) + S(2)*S(2) + S(3)*S(3)

      IF ( L2 .NE. 1.0D0 ) THEN
         POLISH = 1.0D0/DSQRT(L2)
         C      =    C*POLISH
         S(1)   = S(1)*POLISH
         S(2)   = S(2)*POLISH
         S(3)   = S(3)*POLISH
      END IF

      IF ( C .GT. 0.0D0 ) THEN
         Q(0) = C
         Q(1) = S(1)
         Q(2) = S(2)
         Q(3) = S(3)
      ELSE
         Q(0) = -C
         Q(1) = -S(1)
         Q(2) = -S(2)
         Q(3) = -S(3)
      END IF

      RETURN
      END


C
C	$Id$
C
      SUBROUTINE CSSMSURF (N,X,Y,Z,U,LIST,LPTR,LEND,IFLGS,
     .                   SIGMA,W,SM,SMTOL,GSTOL,LPRNT, F,
     .                   GRAD,IER)
      INTEGER N, LIST(*), LPTR(*), LEND(N), IFLGS, LPRNT,
     .        IER
      DOUBLE PRECISION X(N), Y(N), Z(N), U(N), SIGMA(*),
     .                 W(N), SM, SMTOL, GSTOL, F(N),
     .                 GRAD(3,N)
C
C***********************************************************
C
C                                              From SSRFPACK
C                                            Robert J. Renka
C                                  Dept. of Computer Science
C                                       Univ. of North Texas
C                                           renka@cs.unt.edu
C                                                   07/21/98
C
C   Given a triangulation of N nodes on the unit sphere with
C data values U at the nodes and tension factors SIGMA
C associated with the arcs, this routine determines a set of
C nodal function values F and gradients GRAD = (FX,FY,FZ)
C such that a quadratic functional Q1(F,GRAD) is minimized
C subject to the constraint Q2(F) .LE. SM for Q2(F) =
C (U-F)**T*W*(U-F), where W is a diagonal matrix of positive
C weights.  The functional Q1 is an approximation to the
C linearized curvature over the triangulation of a C-1 fun-
C ction F(V), V a unit vector, which interpolates the nodal
C values and gradients.  Subroutines CSINTRC1 and CSUNIF may be
C called to evaluate F at arbitrary points.
C
C   The smoothing procedure is an extension of the method
C for cubic spline smoothing due to C. Reinsch -- Numer.
C Math., 10 (1967) and 16 (1971).  Refer to Function CSFVAL
C for a further description of the interpolant F.  Letting
C D1F(T) and D2F(T) denote first and second derivatives of F
C with respect to a parameter T varying along a triangula-
C tion arc, Q1 is the sum of integrals over the arcs of
C D2F(T)**2 + ((SIGMA/L)*(D1F(T)-S))**2 where L denotes arc-
C length, SIGMA is the appropriate tension factor, and S is
C the slope of the linear function of T which interpolates
C the values of F at the endpoints of the arc.  Introducing
C a smoothing parameter P, and assuming the constraint is
C active, the problem is equivalent to minimizing Q(P,F,
C GRAD) = Q1(F,GRAD) + P*(Q2(F)-SM).  The secant method is
C used to find a zero of G(P) = 1/SQRT(Q2) - 1/SQRT(SM)
C where F(P) satisfies the order-3N symmetric positive def-
C inite linear system obtained by setting the gradient of Q
C (treated as a function of F and GRAD with GRAD tangent to
C the sphere surface) to zero.  The linear system is solved
C by the block Gauss-Seidel method (refer to CSSMSGS).
C
C   Note that the method can also be used to select grad-
C ients for the interpolation problem (F = U, SM = 0, and P
C infinite).  This is achieved by a call to Subroutine
C CSGRADG.
C
C On input:
C
C       N = Number of nodes in the triangulation.  N .GE. 3.
C
C       X,Y,Z = Arrays of length N containing Cartesian
C               coordinates of the nodes.
C
C       U = Array of length N containing data values at the
C           nodes.
C
C       LIST,LPTR,LEND = Data structure defining the trian-
C                        gulation.  Refer to STRIPACK
C                        Subroutine CSTRMESH.
C
C       IFLGS = Tension factor option:
C               IFLGS .LE. 0 if a single uniform tension
C                            factor is to be used.
C               IFLGS .GE. 1 if variable tension is desired.
C
C       SIGMA = Uniform tension factor (IFLGS .LE. 0), or
C               array containing tension factors associated
C               with arcs in one-to-one correspondence with
C               LIST entries (IFLGS .GE. 1).  Refer to Sub-
C               programs CSGETSIG, CSSIG0, CSSIG1, and CSSIG2.
C
C       W = Array of length N containing positive weights
C           associated with the data values.  The recommend-
C           ed value of W(I) is 1/DU**2 where DU is the
C           standard deviation associated with U(I).  DU**2
C           is the expected value of the squared error in
C           the measurement of U(I).  (The mean error is
C           assumed to be zero.)
C
C       SM = Positive parameter specifying an upper bound on
C            Q2(F).  Note that F is constant (and Q2(F)
C            is minimized) if SM is sufficiently large that
C            the constraint is not active.  It is recommend-
C            ed that SM satisfy N-SQRT(2N) .LE. SM .LE. N+
C            SQRT(2N).
C
C       SMTOL = Parameter in the open interval (0,1) speci-
C               fying the relative error allowed in satisfy-
C               ing the constraint -- the constraint is
C               assumed to be satisfied if SM*(1-SMTOL) .LE.
C               Q2 .LE. SM*(1+SMTOL).  A reasonable value
C               for SMTOL is SQRT(2/N).
C
C       GSTOL = Nonnegative tolerance defining the conver-
C               gence criterion for the Gauss-Seidel method.
C               Refer to parameter DFMAX in Subroutine
C               CSSMSGS.  A recommended value is .05*DU**2,
C               where DU is an average standard deviation
C               in the data values.
C
C       LPRNT = Logical unit on which diagnostic messages
C               are printed, or negative integer specifying
C               no diagnostics.  For each secant iteration,
C               the following values are printed:  P, G(P),
C               NIT, DFMAX, and DP, where NIT denotes the
C               number of Gauss-Seidel iterations used in
C               the computation of G, DFMAX denotes the max-
C               imum relative change in a solution component
C               in the last Gauss-Seidel iteration, and DP
C               is the change in P computed by linear inter-
C               polation between the current point (P,G) and
C               a previous point.
C
C Input parameters are not altered by this routine.
C
C On output:
C
C       F = Array of length N containing nodal function val-
C           ues unless IER < 0.
C
C       GRAD = 3 by N array whose columns contain gradients
C              of F at the nodes unless IER < 0.
C
C       IER = Error indicator and information flag:
C             IER = 0 if no errors were encountered and the
C                     constraint is active -- Q2(F) is ap-
C                     proximately equal to SM.
C             IER = 1 if no errors were encountered but the
C                     constraint is not active -- F and GRAD
C                     are the values and gradients of a con-
C                     stant function which minimizes Q2(F),
C                     and Q1 = 0.
C             IER = 2 if the constraint could not be satis-
C                     fied to within SMTOL due to
C                     ill-conditioned linear systems.
C             IER = -1 if N, W, SM, SMTOL, or GSTOL is out-
C                      side its valid range on input.
C             IER = -2 if all nodes are collinear or the
C                      triangulation is invalid.
C             IER = -3 if duplicate nodes were encountered.
C
C SSRFPACK modules required by CSSMSURF:  CSAPLYRT, CSCONSTR,
C                                         CSGRCOEF, CSSMSGS,
C                                         CSSNHCSH
C
C Intrinsic functions called by CSSMSURF:  ABS, SQRT
C
C***********************************************************
C
      INTEGER I, IERR, ITER, ITMAX, LUN, NIT, NITMAX, NN
      DOUBLE PRECISION C, DFMAX, DMAX, DP, G, G0, GNEG, P,
     .                 Q2, Q2MAX, Q2MIN, S, SUMW, TOL, WI
C
C Local parameters:
C
C ITMAX = Maximum number of secant iterations.
C LUN = Local copy of LPRNT.
C NITMAX = Maximum number of Gauss-Seidel iterations for
C          each secant iteration.
C NN = Local copy of N.
C TOL = Local copy of GSTOL.
C
      DATA ITMAX/50/,  NITMAX/40/
C
      NN = N
      TOL = GSTOL
      LUN = LPRNT
      IF (LUN .GT. 99) LUN = -1
C
C Test for errors and initialize F to the weighted least
C   squares fit of a constant function to the data.
C
      IER = -1
      IF (NN .LT. 3  .OR.  SM .LE. 0.  .OR.  SMTOL .LE. 0.
     .    .OR.  SMTOL .GE. 1.  .OR.  TOL .LE. 0.) RETURN
      C = 0.
      SUMW = 0.
      DO 1 I = 1,NN
        WI = W(I)
        IF (WI .LE. 0.) RETURN
        C = C + WI*U(I)
        SUMW = SUMW + WI
    1   CONTINUE
      C = C/SUMW
C
C Compute nodal values and gradients, and accumulate Q2 =
C   (U-F)**T*W*(U-F).
C
      Q2 = 0.
      DO 2 I = 1,NN
        F(I) = C
        GRAD(1,I) = 0.
        GRAD(2,I) = 0.
        GRAD(3,I) = 0.
        Q2 = Q2 + W(I)*(U(I)-F(I))**2
    2   CONTINUE
C
C Compute bounds on Q2 defined by SMTOL, and test for the
C   constraint satisfied by the constant fit.
C
      Q2MIN = SM*(1.-SMTOL)
      Q2MAX = SM*(1.+SMTOL)
      IF (Q2 .LE. Q2MAX) THEN
C
C The constraint is satisfied by a constant function.
C
        IER = 1
        IF (LUN .GE. 0) WRITE (LUN,100)
  100   FORMAT (///1X,'CSSMSURF -- THE CSCONSTRAINT IS NOT ',
     .          'ACTIVE AND THE FITTING FCN IS CONSTANT.')
        RETURN
      ENDIF
C
C Compute G0 = G(0) and print a heading.
C
      IER = 0
      S = 1./SQRT(SM)
      G0 = 1./SQRT(Q2) - S
      IF (LUN .GE. 0) WRITE (LUN,110) SM, TOL, NITMAX, G0
  110 FORMAT (///1X,'CSSMSURF -- SM = ',E10.4,', GSTOL = ',
     .        E7.1,', NITMAX = ',I2,', G(0) = ',E15.8)
C
C G(P) is strictly increasing and concave, and G(0) .LT. 0.
C   Initialize parameters for the secant method.  The method
C   uses three points -- (P0,G0), (P,G), and (PNEG,GNEG)
C   where P0 and PNEG are defined implicitly by DP = P - P0
C   and DMAX = P - PNEG.
C
      P = 10.*SM
      DP = P
      DMAX = 0.
      ITER = 0
C
C Top of loop -- compute G.
C
    3 NIT = NITMAX
      DFMAX = TOL
      CALL CSSMSGS (NN,X,Y,Z,U,LIST,LPTR,LEND,IFLGS,SIGMA,W,
     .            P, NIT,DFMAX,F,GRAD, IERR)
      IF (IERR .LT. 0) IER = IERR
C
C   IERR = -1 in CSSMSGS could be caused by P = 0 as a result
C     of inaccurate solutions to ill-conditioned systems.
C
      IF (IERR .EQ. -1) IER = 2
      IF (IERR .LT. 0) RETURN
      Q2 = 0.
      DO 4 I = 1,NN
        Q2 = Q2 + W(I)*(U(I)-F(I))**2
    4   CONTINUE
      G = 1./SQRT(Q2) - S
      ITER = ITER + 1
      IF (LUN .GE. 0) WRITE (LUN,120) ITER, P, G, NIT, DFMAX
  120 FORMAT (/1X,I2,' -- P = ',E15.8,', G = ',E15.8,
     .        ', NIT = ',I2,', DFMAX = ',E12.6)
C
C   Test for convergence.
C
      IF (Q2MIN .LE. Q2  .AND.  Q2 .LE. Q2MAX) RETURN
      IF (ITER .GE. ITMAX) THEN
        IER = 2
        RETURN
      ENDIF
      IF (DMAX .EQ. 0.  .AND.  G .LE. 0.) THEN
C
C   Increase P until G(P) > 0.
C
        P = 10.*P
        DP = P
        GO TO 3
      ENDIF
C
C   A bracketing interval [P0,P] has been found.
C
      IF (G0*G .LE. 0.) THEN
C
C   G0*G < 0.  Update (PNEG,GNEG) to (P0,G0) so that G
C     and GNEG always have opposite signs.
C
        DMAX = DP
        GNEG = G0
      ENDIF
C
C   Compute the change in P by linear interpolation between
C     (P0,G0) and (P,G).
C
    5 DP = -G*DP/(G-G0)
      IF (LUN .GE. 0) WRITE (LUN,130) DP
  130 FORMAT (1X,5X,'DP = ',E15.8)
      IF (ABS(DP) .GT. ABS(DMAX)) THEN
C
C   G0*G .GT. 0 and the new estimate would be outside of the
C     bracketing interval of length ABS(DMAX).  Reset
C     (P0,G0) to (PNEG,GNEG).
C
        DP = DMAX
        G0 = GNEG
        GO TO 5
      ENDIF
C
C   Bottom of loop -- update P, DMAX, and G0.
C
      P = P + DP
      DMAX = DMAX + DP
      G0 = G
      GO TO 3
      END


MODULE FUN_xyzSDIST
!!#### PURPOSE
!! Compute the signed distance from a plane to a point.

!!#### EXTERNAL KINDS
USE KND_IntrinsicTypes,ONLY: KIND_Rsp,KIND_Rdp !!((01-A-KND_IntrinsicTypes.f90))

!!#### EXTERNAL PROCEDURES
USE FUN_xyzDOT,ONLY: xyzDOT_VV                 !!((05-B-FUN_xyzDOT.f90))

!!#### DEFAULT IMPLICIT
IMPLICIT NONE

!!#### DEFAULT ACCESS
PRIVATE

!!#### PROCEDURE OVERLOADING
INTERFACE xyzSDIST_PnP
 MODULE PROCEDURE xyzSDIST_PnP_Rsp
 MODULE PROCEDURE xyzSDIST_PnP_Rdp
END INTERFACE

!!#### PUBLIC ACCESS LIST
PUBLIC :: xyzSDIST_PnP

CONTAINS

PURE FUNCTION xyzSDIST_PnP_Rsp( Pn , P ) RESULT( SDIST )
!!#### LOCAL KINDS
USE KND_IntrinsicTypes,ONLY: KIND_R=>KIND_Rsp  !!((01-A-KND_IntrinsicTypes.f90))
INCLUDE "06-B-FUN_xyzSDIST_PnP.f90.hdr"
!!--begin--
INCLUDE "06-B-FUN_xyzSDIST_PnP.f90.bdy"
!!--end--
END FUNCTION

PURE FUNCTION xyzSDIST_PnP_Rdp( Pn , P ) RESULT( SDIST )
!!#### LOCAL KINDS
USE KND_IntrinsicTypes,ONLY: KIND_R=>KIND_Rdp  !!((01-A-KND_IntrinsicTypes.f90))
INCLUDE "06-B-FUN_xyzSDIST_PnP.f90.hdr"
!!--begin--
INCLUDE "06-B-FUN_xyzSDIST_PnP.f90.bdy"
!!--end--
END FUNCTION

END MODULE


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

program test

  use iso_fortran_env     , only : dp => REAL64
  use matrix_utils        , only : toepliz
  use direct_linear_solve , only : mgs_qrfactor, cgs_qrfactor, &
       & householder, banded_householder, householder_factorization
  use direct_linear_solve , only : givens_factorization, givens
  use linear_algebra      , only : eigvals, svdvals

  implicit none

  call test_givens_factorization()

contains

  subroutine test_givens_factorization

    real(dp), allocatable :: A(:,:), Q(:,:), R(:,:), D(:,:)
    integer , parameter :: matsize = 800
    integer :: i

    allocate(Q(matsize, matsize))
    allocate(R(matsize, matsize))
    allocate(D(matsize, matsize))
    allocate(A(matsize, matsize))

    call matrix(A)

!!$    A(1,:) = [6.0d0, 5.0d0, 0.0d0]
!!$    A(2,:) = [5.0d0, 1.0d0, 4.0d0]
!!$    A(3,:) = [0.0d0, 4.0d0, 3.0d0]
!!$    print *, 'matrix'
!!$    do i = 1, matsize
!!$       print *, A(i,:)
!!$    end do

    print *, 'Performing Givens Transformation'
    Q = 0
    R = 0
    call givens_factorization(A, Q, R)

    ! Check similarity with A
    print *, 'consistency A'
    D = matmul(Q, R)
    print *, norm2(A - D)

    ! Check Orthogonality
    D = matmul(Q,transpose(Q))
    print *, 'orthogonality Q'
    do i = 1, matsize
       D(i,i) = 1.0d0 - D(i,i)
    end do
    print *, norm2(D)

!!$    print *, 'Upper Triangular Matrix R'
!!$    do i = 1, matsize
!!$       print *, R(i,:)
!!$    end do

    deallocate(A,Q,R,D)

    stop

    test_givens: block

      real(dp) :: x(2), y(2)

      call random_number(x)
      x(1) = 1.0d0
      x(2) = 0.0d0

      !x(2) = tiny(1.0d0)
      print *,x
      call givens(x)
      print *,x
    end block test_givens

  end subroutine test_givens_factorization

  !===================================================================!
  ! Setup Toepliz matrix with supplied columns
  !===================================================================!

  subroutine matrix(A)

    real(8), allocatable :: A(:,:)
    real(8), allocatable :: diag(:)
    integer :: m, j

    A = 0
    call random_number(A(:,1))
    call random_number(A(1,:))
    m = size(A, 1)

    allocate(diag(m))
    call random_number(diag)
    do concurrent(j=1:m)
       A(j,j) = diag(j)
    end do
    deallocate(diag)

  end subroutine matrix

  subroutine test_cgs_mgs_householder

    integer, parameter :: n  = 200

    ! integer, parameter :: sizes(10) = [100,200,300,400,500,600,700,800,900,1000]
    ! integer, parameter :: sizes(7) = [100,200,400,800,1600,3200,6400]

    real(8), allocatable :: A(:,:), B(:,:)
    real(8), allocatable :: Q(:,:), R(:,:)
    real(8), allocatable :: U(:,:), V(:,:), S(:,:), RC(:,:), RM(:,:)
    real(8), allocatable :: svda(:)

    real(8) :: walltime1, walltime2, time_end, time_Start
    integer :: i, info

    ! Allocte matrices
    allocate(A(n,n), B(n,n), Q(n,n), R(n,n))
    allocate(U(n,n), V(n,n), S(n,n), RC(n,n), RM(n,n))
    U = 0
    V = 0
    S = 0
    allocate(svda(n))

    ! Form a matrix with required singular values
    call random_number(A)
    call mgs_qrfactor(A, U, R, info)

    call random_number(A)
    call mgs_qrfactor(A, V, R, info)
    do i = 1, n
       S(i,i) = 3.0d0**dble(-i)
    end do
    A = matmul(U,matmul(S,V))

    print *, 'performing classical GS on A'
    call cgs_qrfactor(A, Q, RC, info)
    S = matmul(Q,transpose(Q))
    do i = 1, n
       S(i,i) = 1.0d0 - S(i,i)
       print *, S(i,i)
    end do

    ! Perform orthogonality check
    print *, n, norm2(S), norm2(A-matmul(Q,RC))

    print *, 'performing modified GS on A'
    call mgs_qrfactor(A, Q, RM, info)
    S = matmul(Q,transpose(Q))
    do i = 1, n
       S(i,i) = 1.0d0 - S(i,i)
       print *, S(i,i)
    end do

    ! Perform orthogonality check
    print *, n, norm2(S), norm2(A-matmul(Q,RM))

    print *, 'Householder'
    Q = 0
    R = 0
    !call householder(A)
    call householder_factorization(A, Q, R)
    S = matmul(Q,transpose(Q))
    do i = 1, n
       S(i,i) = 1.0d0 - S(i,i)
       print *, S(i,i)
    end do

    print *, "index ", "cgs ", "mgs ", "householder"
    do i = 1, n
       print *, i, RC(i,i), RM(i,i), abs(R(i,i))
    end do

    stop

    ! system 2
!!$
!!$  print *, "npts ", "walltime1 ", "walltime2"
!!$
!!$  do i = 1, size(sizes)
!!$
!!$     n = sizes(i)
!!$
!!$     allocate(svda(n))
!!$     allocate(A(n,n), Q(n,n), R(n,n))
!!$     svda = 0
!!$     A = 0
!!$     Q = 0
!!$     R = 0
!!$
!!$     svda(1) = 10.0d0
!!$     svda(2) = 3.0d0
!!$     svda(3) = 2.0d0
!!$     svda(4) = 1.0d0
!!$
!!$     call toepliz(svda, A)
!!$
!!$     call cpu_time(time_start)
!!$     call householder(A)
!!$     call cpu_time(time_end)
!!$     walltime1 = time_end - time_start
!!$
!!$     A = 0
!!$     call toepliz(svda, A)
!!$
!!$     call cpu_time(time_start)
!!$     call banded_householder(A,3,3)
!!$     call cpu_time(time_end)
!!$     walltime2 = time_end - time_start
!!$
!!$     print *, n, walltime1, walltime2
!!$
!!$
!!$     deallocate(svda, A, Q, R)
!!$
!!$  end do
!!$
!!$  !do i = 1, n
!!$  !   write(*,*) i, A(i,:)
!!$  !end do
!!$
!!$  stop
!!$  print *, 'performing classical GS on A'
!!$
!!$  !call mgs_qrfactor(A, Q, R, info)
!!$  !call banded_householder(A, 2, 2)
!!$
!!$  !print *, A- matmul(Q,R)
!!$  do i = 1, n
!!$     print *, i, A(i,i)
!!$  end do
!!$

  end subroutine test_cgs_mgs_householder

end program test


      double precision function dmach(job)
      integer job
c
c     smach computes machine parameters of floating point
c     arithmetic for use in testing only.  not required by
c     linpack proper.
c
c     if trouble with automatic computation of these quantities,
c     they can be set by direct assignment statements.
c     assume the computer has
c
c        b = base of arithmetic
c        t = number of base  b  digits
c        l = smallest possible exponent
c        u = largest possible exponent
c
c     then
c
c        eps = b**(1-t)
c        tiny = 100.0*b**(-l+t)
c        huge = 0.01*b**(u-t)
c
c     dmach same as smach except t, l, u apply to
c     double precision.
c
c     cmach same as smach except if complex division
c     is done by
c
c        1/(x+i*y) = (x-i*y)/(x**2+y**2)
c
c     then
c
c        tiny = sqrt(tiny)
c        huge = sqrt(huge)
c
c
c     job is 1, 2 or 3 for epsilon, tiny and huge, respectively.
c
      double precision eps,tiny,huge,s
c
      eps = 1.0d0
   10 eps = eps/2.0d0
      s = 1.0d0 + eps
      if (s .gt. 1.0d0) go to 10
      eps = 2.0d0*eps
c
      s = 1.0d0
   20 tiny = s
      s = s/16.0d0
      if (s*1.0 .ne. 0.0d0) go to 20
      tiny = (tiny/eps)*100.0
      huge = 1.0d0/tiny
c
      if (job .eq. 1) dmach = eps
      if (job .eq. 2) dmach = tiny
      if (job .eq. 3) dmach = huge
      return
      end


program cinematica

implicit none

real, dimension (1:100) :: x, t, v
real :: xo, c, vo, a
integer :: i

xo = 1
vo = 5
a = 2

c = 0
do i = 1,100
    c = c + 0.5
    t(i) = c
    v(i) = vo + a*t(i)
    x(i) = xo + vo*t(i) + (1d0/2d0)*a*t(i)**2
end do

do i = 1,100
    write(*,*) t(i), x(i), v(i)
end do

open(1,FILE = "dados.txt")

do i = 1,100
    write(1,*) t(i), x(i), v(i)
end do

close(1)

end program cinematica


PROGRAM test_ave_value2
!
!  Purpose:
!    To test function ave_value by calling it with the intrinsic
!    function sin.
!
!  Record of revisions:
!      Date       Programmer          Description of change
!      ====       ==========          =====================
!    12/31/06    S. J. Chapman        Original code
!
IMPLICIT NONE

! Declare functions:
REAL :: ave_value              ! Average value of function
INTRINSIC sin                  ! Function to evaluate

! Declare parameters:
REAL, PARAMETER :: TWOPI = 6.283185 ! 2 * Pi

! Declare local variables:
REAL :: ave                    ! Average of my_function

! Call function with func=sin.
ave = ave_value ( sin, 0., TWOPI, 101 )
WRITE (*,1000) 'SIN', ave
1000 FORMAT (1X,'The average value of ',A,' between 0. and twopi is ', &
             F16.6,'.')

END PROGRAM test_ave_value2


  program analytical_solution

! This program implements the analytical solution for the displacement vector in a 3D viscoelastic medium
! with a vertical force source located in (0,0,0).
! Implemented by Dimitri Komatitsch from the 2D plane-strain viscoelastic medium analytical solution
! of Appendix B of Carcione et al., Wave propagation simulation in a linear viscoelastic medium, GJI, vol. 95, p. 597-611 (1988)
! (note that that Appendix contains two typos, fixed in this code).
! The amplitude of the force is called F and is defined below.

!! DK DK Dimitri Komatitsch, CNRS Marseille, France, May 2018.

  implicit none

!! DK DK May 2018: the missing 1/L factor in older Carcione papers
!! DK DK May 2018: has been added to this code by Quentin Brissaud and by Etienne Bachmann
!! DK DK for the viscoacoustic code in directory EXAMPLES/attenuation/viscoacoustic,
!! DK DK it would be very easy to copy the changes from there to this viscoelastic version;
!! DK DK but then all the values of the tau_epsilon in the code below would need to change.

!! DK DK Dimitri Komatitsch, CNRS Marseille, France, April 2017: added the elastic reference calculation.

! compute the non-viscoacoustic case as a reference if needed, i.e. turn attenuation off
  logical, parameter :: TURN_ATTENUATION_OFF = .true.

! to see how small the contribution of the near-field term is,
! here the user can ask not to include it, to then compare with the full result obtained with this flag set to false
  logical, parameter :: DO_NOT_COMPUTE_THE_NEAR_FIELD = .false.

  integer, parameter :: iratio = 32

  integer, parameter :: nfreq = 524288
  integer, parameter :: nt = iratio * nfreq

  double precision, parameter :: freqmax = 80.d0
!! DK DK to print the velocity if we want to display the curve of how velocity varies with frequency
!! DK DK for instance to compute the unrelaxed velocity in the Zener model
! double precision, parameter :: freqmax = 20000.d0

  double precision, parameter :: freqseuil = 0.00005d0

  double precision, parameter :: pi = 3.141592653589793d0

! for the solution in time domain
  integer it,i
  real wsave(4*nt+15)
  complex c(nt)

!! DK DK for my slow inverse Discrete Fourier Transform using a double loop
  complex :: input(nt), i_imaginary_constant
  integer :: j,m

! density of the medium
  double precision, parameter :: rho = 2000.d0

! amplitude of the force source
  double precision, parameter :: F = 1.d0

! spatial dimension of the problem (this analytical code is valid only in three dimensions, thus do not change this)
  integer, parameter :: NDIM = 3

! definition position recepteur Carcione
  double precision, dimension(NDIM) :: x

! Definition source Dimitri
  double precision, parameter :: f0 = 18.d0
  double precision, parameter :: t0 = 1.2d0 / f0

! Definition source Carcione
! double precision f0,t0,eta,epsil
! parameter(f0 = 50.d0)
! parameter(t0 = 0.075d0)
! parameter(epsil = 1.d0)
! parameter(eta = 0.5d0)

! number of Zener standard linear solids in parallel
  integer, parameter :: Lnu = 3

! DK DK I implemented a very simple and slow inverse Discrete Fourier Transform
! DK DK at some point, for verification, using a double loop. I keep it just in case.
! DK DK For large number of points it is extremely slow because of the double loop.
! DK DK Thus there is no reason to turn this flag on.
  logical, parameter :: USE_SLOW_FOURIER_TRANSFORM = .false.

!! DK DK March 2018: this missing 1/L factor has been added to this code by Quentin Brissaud
!! DK DK for the viscoacoustic code in directory EXAMPLES/attenuation/viscoacoustic,
!! DK DK it would be very easy to copy the changes from there to this viscoelastic version;
!! DK DK but then all the values of the tau_epsilon below would need to change.

 double precision, dimension(Lnu) :: tau_sigma_kappa,tau_sigma_mu,tau_epsilon_kappa,tau_epsilon_mu

  integer :: ifreq
  double precision :: deltafreq,freq,omega,omega0,deltat,time,a
  double complex :: comparg

! Fourier transform of the Ricker wavelet source
  double complex fomega(0:nfreq)

! real and imaginary parts
  double precision ra(0:nfreq),rb(0:nfreq)

! spectral amplitude
  double precision ampli(0:nfreq)

! analytical solution for the three components
  double complex phi1(-nfreq:nfreq)
  double complex phi2(-nfreq:nfreq)
  double complex phi3(-nfreq:nfreq)

! external function
  double complex, external :: ui

! modules elastiques
  double complex :: Kappa_omega, Mu_omega, Vp_omega, Vs_omega, temp, Mu_relaxed, Kappa_relaxed

! ********** end of variable declarations ************

!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!
!  Below are all the variables that need to be obtained from Specfem3D
!  These default values match the following simulation:
!
!  Vp(f0_ref) = 3297.849 m/s
!  Vs(f0_ref) = 2222.536 m/s
!  f0_ref = 18 Hz
!  QKappa = 9000
!  QMu = 10
!
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

! We use Kappa_ref and Mu_ref to compute the case without attenuation
! They are chosen such that Kappa_ref = Kappa(2*pi*f0_ref) when there is
! attenuation
  double precision, parameter :: Vp_ref  = 3297.849
  double precision, parameter :: Vs_ref  = 2222.536

  double precision, parameter :: Mu_ref    =  rho * Vs_ref * Vs_ref
  double precision, parameter :: Kappa_ref =  rho * ( Vp_ref * Vp_ref - 4.d0/3 * Vs_ref * Vs_ref)

! add this for the case of an elastic medium, with viscoelasticity turned off
  double precision, parameter :: Mu_unrelaxed    =  Mu_ref
  double precision, parameter :: Kappa_unrelaxed =  Kappa_ref

! tau constants mimicking constant QKappa and KMu
  tau_epsilon_mu    = (/ 0.281966668348107  ,  3.607809663879573E-002 , 5.638875613224542E-003/)
  tau_sigma_mu      = (/ 0.186873539567019  ,  2.491998701168405E-002 , 3.323133676931235E-003/)
  tau_epsilon_kappa = (/ 0.186973292921151  ,  2.492998179955646E-002 , 3.324907855424433E-003/)
  tau_sigma_kappa   = (/ 0.186873539567019  ,  2.491998701168405E-002 , 3.323133676931235E-003/)

! Eq (32) of Jeroen's note, eq (2.199) of Carcione's book from 2014, third edition
  Kappa_relaxed = (Kappa_unrelaxed /(sum(tau_epsilon_kappa(:)/tau_sigma_kappa(:))/Lnu))
  Mu_relaxed    = (Mu_unrelaxed    /(sum(tau_epsilon_mu(:)/tau_sigma_mu(:))/Lnu))

! position of the receiver
  x(1) = +500.
  x(2) = +500.
  x(3) = +500.

  print *,'Force source located at the origin (0,0,0)'
  print *,'Receiver located in (x,y,z) = ',x(1),x(2),x(3)

  if (TURN_ATTENUATION_OFF) then
    print *,'BEWARE: computing the elastic reference solution (i.e., without attenuation) instead of the viscoelastic solution'
  else
    print *,'Computing the viscoelastic solution'
  endif

  if (DO_NOT_COMPUTE_THE_NEAR_FIELD) then
    print *,'BEWARE: computing the far-field solution only, rather than the full Green function'
  else
    print *,'Computing the full solution, including the near-field term of the Green function'
  endif

! step in frequency
  deltafreq = freqmax / dble(nfreq)

! define parameters for the Ricker source
  omega0 = 2.d0 * pi * f0
  a = pi**2 * f0**2

  deltat = 1.d0 / (freqmax*dble(iratio))

! define the spectrum of the source
  do ifreq=0,nfreq
      freq = deltafreq * dble(ifreq)
      omega = 2.d0 * pi * freq

! typo in equation (B7) of Carcione et al., Wave propagation simulation in a linear viscoelastic medium,
! Geophysical Journal, vol. 95, p. 597-611 (1988), the exponential should be of -i omega t0,
! fixed here by adding the minus sign
      comparg = dcmplx(0.d0,-omega*t0)

! definir le spectre du Ricker de Carcione avec cos()
! equation (B7) of Carcione et al., Wave propagation simulation in a linear viscoelastic medium,
! Geophysical Journal, vol. 95, p. 597-611 (1988)
!     fomega(ifreq) = pi * dsqrt(pi/eta) * (1.d0/omega0) * cdexp(comparg) * ( dexp(- (pi*pi/eta) * (epsil/2 - omega/omega0)**2) &
!         + dexp(- (pi*pi/eta) * (epsil/2 + omega/omega0)**2) )

! definir le spectre d'un Ricker classique (centre en t0)
      fomega(ifreq) = dsqrt(pi) * cdexp(comparg) * omega**2 * dexp(-omega**2/(4.d0*a)) / (2.d0 * dsqrt(a**3))

      ra(ifreq) = dreal(fomega(ifreq))
      rb(ifreq) = dimag(fomega(ifreq))
! prendre le module de l'amplitude spectrale
      ampli(ifreq) = dsqrt(ra(ifreq)**2 + rb(ifreq)**2)
  enddo

! sauvegarde du spectre d'amplitude de la source en Hz au format Gnuplot
  open(unit=10,file='spectrum_of_the_source_used.gnu',status='unknown')
  do ifreq = 0,nfreq
    freq = deltafreq * dble(ifreq)
    write(10,*) sngl(freq),sngl(ampli(ifreq))
  enddo
  close(10)

! ************** calcul solution analytique ****************

! d'apres Carcione GJI vol 95 p 611 (1988)
  do ifreq=0,nfreq
      freq = deltafreq * dble(ifreq)
      omega = 2.d0 * pi * freq

! critere ad-hoc pour eviter singularite en zero
  if (freq < freqseuil) omega = 2.d0 * pi * freqseuil

! use standard infinite frequency (unrelaxed) reference,
! in which waves slow down when attenuation is turned on.
  temp = dcmplx(0.d0,0.d0)
  do i=1,Lnu
    temp = temp + (1./Lnu)*dcmplx(1.d0,omega*tau_epsilon_kappa(i)) / dcmplx(1.d0,omega*tau_sigma_kappa(i))
  enddo

! Eq(31) of Jeroen's note
  Kappa_omega = Kappa_relaxed * temp

  temp = dcmplx(0.d0,0.d0)
  do i=1,Lnu
    temp = temp + (1./Lnu)*dcmplx(1.d0,omega*tau_epsilon_mu(i)) / dcmplx(1.d0,omega*tau_sigma_mu(i))
  enddo

! Eq (31) of Jeroen's note
  Mu_omega = Mu_relaxed * temp

  if (TURN_ATTENUATION_OFF) then
! wave speeds and moduli are the same for all omega when there is no attenuation
    Kappa_omega = Kappa_ref
    Mu_omega    = Mu_ref
  endif

  Vs_omega = cdsqrt( Mu_omega / rho)
  Vp_omega = cdsqrt( Kappa_omega / rho + (4.d0/3)*Vs_omega*Vs_omega )

!! DK DK print the velocity if we want to display the curve of how velocity varies with frequency
!! DK DK for instance to compute the unrelaxed velocity in the Zener model
! print *,freq,dsqrt(real(Vp_omega)**2 + imag(Vp_omega)**2)
!! DK DK print the velocity if we want to display the curve of how velocity varies with frequency
!! DK DK for instance to compute the unrelaxed velocity in the Zener model
! print *,freq,dsqrt(real(Vs_omega)**2 + imag(Vs_omega)**2)

! calcul de la solution analytique en frequence
  phi1(ifreq) = ui(1,omega,Vp_omega,Vs_omega,x,rho,NDIM,F,DO_NOT_COMPUTE_THE_NEAR_FIELD) * fomega(ifreq)
  phi2(ifreq) = ui(2,omega,Vp_omega,Vs_omega,x,rho,NDIM,F,DO_NOT_COMPUTE_THE_NEAR_FIELD) * fomega(ifreq)
  phi3(ifreq) = ui(3,omega,Vp_omega,Vs_omega,x,rho,NDIM,F,DO_NOT_COMPUTE_THE_NEAR_FIELD) * fomega(ifreq)

  enddo

! take the conjugate value for negative frequencies
  do ifreq=-nfreq,-1
      phi1(ifreq) = dconjg(phi1(-ifreq))
      phi2(ifreq) = dconjg(phi2(-ifreq))
      phi3(ifreq) = dconjg(phi3(-ifreq))
  enddo

! ***************************************************************************
! Calculation of the time domain solution (using routine "cfftb" from Netlib)
! ***************************************************************************

! **********
! Compute Ux
! **********

! initialize FFT arrays
  call cffti(nt,wsave)

! clear array of Fourier coefficients
  do it = 1,nt
    c(it) = cmplx(0.,0.)
  enddo

! use the Fourier values for Ux
  c(1) = cmplx(phi1(0))
  do ifreq=1,nfreq-2
      c(ifreq+1) = cmplx(phi1(ifreq))
      c(nt+1-ifreq) = conjg(cmplx(phi1(ifreq)))
  enddo

! perform the inverse FFT for Ux
  if (.not. USE_SLOW_FOURIER_TRANSFORM) then
    call cfftb(nt,c,wsave)
  else
! DK DK I implemented a very simple and slow inverse Discrete Fourier Transform here
! DK DK at some point, for verification, using a double loop. I keep it just in case.
! DK DK For large number of points it is extremely slow because of the double loop.
    input(:) = c(:)
!   imaginary constant "i"
    i_imaginary_constant = (0.,1.)
    do it = 1,nt
      if (mod(it,1000) == 0) print *,'FFT inverse it = ',it,' out of ',nt
      j = it
      c(j) = cmplx(0.,0.)
      do m = 1,nt
        c(j) = c(j) + input(m) * exp(2.d0 * PI * i_imaginary_constant * dble((m-1) * (j-1)) / nt)
      enddo
    enddo
  endif

! in the inverse Discrete Fourier transform one needs to divide by N, the number of samples (number of time steps here)
  c(:) = c(:) / nt

! value of a time step
  deltat = 1.d0 / (freqmax*dble(iratio))

! to get the amplitude right, we need to divide by the time step
  c(:) = c(:) / deltat

! save time result inverse FFT for Ux

  if (TURN_ATTENUATION_OFF) then
    if (DO_NOT_COMPUTE_THE_NEAR_FIELD) then
      open(unit=11,file='Ux_time_analytical_solution_elastic_without_near_field.dat',status='unknown')
    else
      open(unit=11,file='Ux_time_analytical_solution_elastic.dat',status='unknown')
    endif
  else
    if (DO_NOT_COMPUTE_THE_NEAR_FIELD) then
      open(unit=11,file='Ux_time_analytical_solution_viscoelastic_without_near_field.dat',status='unknown')
    else
      open(unit=11,file='Ux_time_analytical_solution_viscoelastic.dat',status='unknown')
    endif
  endif
  do it=1,nt
! DK DK Dec 2011: subtract t0 to be consistent with the SPECFEM2D code
        time = dble(it-1)*deltat - t0
! the seismograms are very long due to the very large number of FFT points used,
! thus keeping the useful part of the signal only (the first six seconds of the seismogram)
        if (time >= 0.d0 .and. time <= 6.d0) write(11,*) sngl(time),real(c(it))
  enddo
  close(11)

! **********
! Compute Uy
! **********

! clear array of Fourier coefficients
  do it = 1,nt
    c(it) = cmplx(0.,0.)
  enddo

! use the Fourier values for Uy
  c(1) = cmplx(phi2(0))
  do ifreq=1,nfreq-2
      c(ifreq+1) = cmplx(phi2(ifreq))
      c(nt+1-ifreq) = conjg(cmplx(phi2(ifreq)))
  enddo

! perform the inverse FFT for Uy
  if (.not. USE_SLOW_FOURIER_TRANSFORM) then
    call cfftb(nt,c,wsave)
  else
! DK DK I implemented a very simple and slow inverse Discrete Fourier Transform here
! DK DK at some point, for verification, using a double loop. I keep it just in case.
! DK DK For large number of points it is extremely slow because of the double loop.
    input(:) = c(:)
!   imaginary constant "i"
    i_imaginary_constant = (0.,1.)
    do it = 1,nt
      if (mod(it,1000) == 0) print *,'FFT inverse it = ',it,' out of ',nt
      j = it
      c(j) = cmplx(0.,0.)
      do m = 1,nt
        c(j) = c(j) + input(m) * exp(2.d0 * PI * i_imaginary_constant * dble((m-1) * (j-1)) / nt)
      enddo
    enddo
  endif

! in the inverse Discrete Fourier transform one needs to divide by N, the number of samples (number of time steps here)
  c(:) = c(:) / nt

! value of a time step
  deltat = 1.d0 / (freqmax*dble(iratio))

! to get the amplitude right, we need to divide by the time step
  c(:) = c(:) / deltat

! save time result inverse FFT for Uy
  if (TURN_ATTENUATION_OFF) then
    if (DO_NOT_COMPUTE_THE_NEAR_FIELD) then
      open(unit=11,file='Uy_time_analytical_solution_elastic_without_near_field.dat',status='unknown')
    else
      open(unit=11,file='Uy_time_analytical_solution_elastic.dat',status='unknown')
    endif
  else
    if (DO_NOT_COMPUTE_THE_NEAR_FIELD) then
      open(unit=11,file='Uy_time_analytical_solution_viscoelastic_without_near_field.dat',status='unknown')
    else
      open(unit=11,file='Uy_time_analytical_solution_viscoelastic.dat',status='unknown')
    endif
  endif
  do it=1,nt
! DK DK Dec 2011: subtract t0 to be consistent with the SPECFEM2D code
        time = dble(it-1)*deltat - t0
! the seismograms are very long due to the very large number of FFT points used,
! thus keeping the useful part of the signal only (the first six seconds of the seismogram)
        if (time >= 0.d0 .and. time <= 6.d0) write(11,*) sngl(time),real(c(it))
  enddo
  close(11)

! **********
! Compute Uz
! **********

! clear array of Fourier coefficients
  do it = 1,nt
    c(it) = cmplx(0.,0.)
  enddo

! use the Fourier values for Uz
  c(1) = cmplx(phi3(0))
  do ifreq=1,nfreq-2
      c(ifreq+1) = cmplx(phi3(ifreq))
      c(nt+1-ifreq) = conjg(cmplx(phi3(ifreq)))
  enddo

! perform the inverse FFT for Uz
  if (.not. USE_SLOW_FOURIER_TRANSFORM) then
    call cfftb(nt,c,wsave)
  else
! DK DK I implemented a very simple and slow inverse Discrete Fourier Transform here
! DK DK at some point, for verification, using a double loop. I keep it just in case.
! DK DK For large number of points it is extremely slow because of the double loop.
    input(:) = c(:)
!   imaginary constant "i"
    i_imaginary_constant = (0.,1.)
    do it = 1,nt
      if (mod(it,1000) == 0) print *,'FFT inverse it = ',it,' out of ',nt
      j = it
      c(j) = cmplx(0.,0.)
      do m = 1,nt
        c(j) = c(j) + input(m) * exp(2.d0 * PI * i_imaginary_constant * dble((m-1) * (j-1)) / nt)
      enddo
    enddo
  endif

! in the inverse Discrete Fourier transform one needs to divide by N, the number of samples (number of time steps here)
  c(:) = c(:) / nt

! value of a time step
  deltat = 1.d0 / (freqmax*dble(iratio))

! to get the amplitude right, we need to divide by the time step
  c(:) = c(:) / deltat

! save time result inverse FFT for Uz
  if (TURN_ATTENUATION_OFF) then
    if (DO_NOT_COMPUTE_THE_NEAR_FIELD) then
      open(unit=11,file='Uz_time_analytical_solution_elastic_without_near_field.dat',status='unknown')
    else
      open(unit=11,file='Uz_time_analytical_solution_elastic.dat',status='unknown')
    endif
  else
    if (DO_NOT_COMPUTE_THE_NEAR_FIELD) then
      open(unit=11,file='Uz_time_analytical_solution_viscoelastic_without_near_field.dat',status='unknown')
    else
      open(unit=11,file='Uz_time_analytical_solution_viscoelastic.dat',status='unknown')
    endif
  endif
  do it=1,nt
! DK DK Dec 2011: subtract t0 to be consistent with the SPECFEM2D code
        time = dble(it-1)*deltat - t0
! the seismograms are very long due to the very large number of FFT points used,
! thus keeping the useful part of the signal only (the first six seconds of the seismogram)
        if (time >= 0.d0 .and. time <= 6.d0) write(11,*) sngl(time),real(c(it))
  enddo
  close(11)

  end

! -----------

  double complex function ui(i,omega,v1,v2,x,rho,NDIM,F,DO_NOT_COMPUTE_THE_NEAR_FIELD)

  implicit none

  double precision, parameter :: pi = 3.141592653589793d0

  integer i,j
  double precision omega,F
  double complex :: v1,v2,Up_far_field,Us_far_field,near_field,i_imaginary_constant
  double complex :: fourier_transform_of_Heaviside_times_t_for_Vp,fourier_transform_of_Heaviside_times_t_for_Vs
  logical :: DO_NOT_COMPUTE_THE_NEAR_FIELD

  integer :: NDIM,kronecker_delta_symbol
  double precision, dimension(NDIM) :: x,gamma_vector
  double precision :: r,rho

! source-receiver distance
  r = dsqrt(x(1)**2 + x(2)**2 + x(3)**2)

! define the gamma vector of Aki and Richards (1980), which is the unit vector from the source to the receiver
! see Aki and Richards (1980) below Box 4.3 (continued) and above equation (4.23)
  gamma_vector(:) = x(:) / r

! the force is vertical in this analytical code, thus the j direction of the force is Z
  j = 3

! imaginary constant "i"
  i_imaginary_constant = (0.d0,1.d0)

! Kronecker delta symbol
  if (i == j) then
    kronecker_delta_symbol = 1
  else
    kronecker_delta_symbol = 0
  endif

! far-field P wave term
! see e.g. Aki and Richards (1980), equation (4.24) in the time domain; here we use the same equation but in the frequency domain
  Up_far_field = gamma_vector(i) * gamma_vector(j) * exp(-i_imaginary_constant*omega*r/v1) / (4.d0 * pi * rho * v1**2 * r)

! far-field S wave term
! see e.g. Aki and Richards (1980), equation (4.25) in the time domain; here we use the same equation but in the frequency domain
! note that Aki and Richards (1980) has a typo in equation (4.25), a term 1/r is missing; fixed here.
! The corrected equation is for instance in Jose Pujol, Elastic wave propagation and generation in seismology, equation (9.6.1)
  Us_far_field = (kronecker_delta_symbol - gamma_vector(i) * gamma_vector(j)) * exp(-i_imaginary_constant*omega*r/v2) &
               / (4.d0 * pi * rho * v2**2 * r)

! near field term (see e.g. Jose Pujol, Elastic wave propagation and generation in seismology, equation (9.6.1) in the time domain;
! here we use the same equation but in the frequency domain)
  fourier_transform_of_Heaviside_times_t_for_Vp = i_imaginary_constant * (-omega*r + i_imaginary_constant*v1) * &
                   exp(-i_imaginary_constant*omega*r/v1) / (v1 * omega**2)

  fourier_transform_of_Heaviside_times_t_for_Vs = i_imaginary_constant * (-omega*r + i_imaginary_constant*v2) * &
                   exp(-i_imaginary_constant*omega*r/v2) / (v2 * omega**2)

  near_field = (3 * gamma_vector(i) * gamma_vector(j) - kronecker_delta_symbol) * &
     (fourier_transform_of_Heaviside_times_t_for_Vp - fourier_transform_of_Heaviside_times_t_for_Vs) / (4.d0 * pi * rho * r**3)

  if (DO_NOT_COMPUTE_THE_NEAR_FIELD) near_field = dcmplx(0.d0,0.d0)

! the result is the sum of the three terms; we also multiply by the amplitude of the force
  ui = F * (Up_far_field + Us_far_field + near_field)

  end

! ***************** routine de FFT pour signal en temps ****************

! FFT routine taken from Netlib

  subroutine CFFTB (N,C,WSAVE)
  DIMENSION       C(1)       ,WSAVE(1)
  if (N == 1) return
  IW1 = N+N+1
  IW2 = IW1+N+N
  CALL CFFTB1 (N,C,WSAVE,WSAVE(IW1),WSAVE(IW2))
  return
  END
  subroutine CFFTB1 (N,C,CH,WA,IFAC)
  DIMENSION       CH(1)      ,C(1)       ,WA(1)      ,IFAC(1)
  NF = IFAC(2)
  NA = 0
  L1 = 1
  IW = 1
  DO 116 K1=1,NF
   IP = IFAC(K1+2)
   L2 = IP*L1
   IDO = N/L2
   IDOT = IDO+IDO
   IDL1 = IDOT*L1
   if (IP /= 4) goto 103
   IX2 = IW+IDOT
   IX3 = IX2+IDOT
   if (NA /= 0) goto 101
   CALL PASSB4 (IDOT,L1,C,CH,WA(IW),WA(IX2),WA(IX3))
   goto 102
  101    CALL PASSB4 (IDOT,L1,CH,C,WA(IW),WA(IX2),WA(IX3))
  102    NA = 1-NA
   goto 115
  103    if (IP /= 2) goto 106
   if (NA /= 0) goto 104
   CALL PASSB2 (IDOT,L1,C,CH,WA(IW))
   goto 105
  104    CALL PASSB2 (IDOT,L1,CH,C,WA(IW))
  105    NA = 1-NA
   goto 115
  106    if (IP /= 3) goto 109
   IX2 = IW+IDOT
   if (NA /= 0) goto 107
   CALL PASSB3 (IDOT,L1,C,CH,WA(IW),WA(IX2))
   goto 108
  107    CALL PASSB3 (IDOT,L1,CH,C,WA(IW),WA(IX2))
  108    NA = 1-NA
   goto 115
  109    if (IP /= 5) goto 112
   IX2 = IW+IDOT
   IX3 = IX2+IDOT
   IX4 = IX3+IDOT
   if (NA /= 0) goto 110
   CALL PASSB5 (IDOT,L1,C,CH,WA(IW),WA(IX2),WA(IX3),WA(IX4))
   goto 111
  110    CALL PASSB5 (IDOT,L1,CH,C,WA(IW),WA(IX2),WA(IX3),WA(IX4))
  111    NA = 1-NA
   goto 115
  112    if (NA /= 0) goto 113
   CALL PASSB (NAC,IDOT,IP,L1,IDL1,C,C,C,CH,CH,WA(IW))
   goto 114
  113    CALL PASSB (NAC,IDOT,IP,L1,IDL1,CH,CH,CH,C,C,WA(IW))
  114    if (NAC /= 0) NA = 1-NA
  115    L1 = L2
   IW = IW+(IP-1)*IDOT
  116 continue
  if (NA == 0) return
  N2 = N+N
  DO 117 I=1,N2
   C(I) = CH(I)
  117 continue
  return
  END
  subroutine PASSB (NAC,IDO,IP,L1,IDL1,CC,C1,C2,CH,CH2,WA)
  DIMENSION       CH(IDO,L1,IP)          ,CC(IDO,IP,L1), &
                  C1(IDO,L1,IP)          ,WA(1)      ,C2(IDL1,IP), &
                  CH2(IDL1,IP)
  IDOT = IDO/2
  NT = IP*IDL1
  IPP2 = IP+2
  IPPH = (IP+1)/2
  IDP = IP*IDO
!
  if (IDO < L1) goto 106
  DO 103 J=2,IPPH
   JC = IPP2-J
   DO 102 K=1,L1
      DO 101 I=1,IDO
         CH(I,K,J) = CC(I,J,K)+CC(I,JC,K)
         CH(I,K,JC) = CC(I,J,K)-CC(I,JC,K)
  101       continue
  102    continue
  103 continue
  DO 105 K=1,L1
   DO 104 I=1,IDO
      CH(I,K,1) = CC(I,1,K)
  104    continue
  105 continue
  goto 112
  106 DO 109 J=2,IPPH
   JC = IPP2-J
   DO 108 I=1,IDO
      DO 107 K=1,L1
         CH(I,K,J) = CC(I,J,K)+CC(I,JC,K)
         CH(I,K,JC) = CC(I,J,K)-CC(I,JC,K)
  107       continue
  108    continue
  109 continue
  DO 111 I=1,IDO
   DO 110 K=1,L1
      CH(I,K,1) = CC(I,1,K)
  110    continue
  111 continue
  112 IDL = 2-IDO
  INC = 0
  DO 116 L=2,IPPH
   LC = IPP2-L
   IDL = IDL+IDO
   DO 113 IK=1,IDL1
      C2(IK,L) = CH2(IK,1)+WA(IDL-1)*CH2(IK,2)
      C2(IK,LC) = WA(IDL)*CH2(IK,IP)
  113    continue
   IDLJ = IDL
   INC = INC+IDO
   DO 115 J=3,IPPH
      JC = IPP2-J
      IDLJ = IDLJ+INC
      if (IDLJ > IDP) IDLJ = IDLJ-IDP
      WAR = WA(IDLJ-1)
      WAI = WA(IDLJ)
      DO 114 IK=1,IDL1
         C2(IK,L) = C2(IK,L)+WAR*CH2(IK,J)
         C2(IK,LC) = C2(IK,LC)+WAI*CH2(IK,JC)
  114       continue
  115    continue
  116 continue
  DO 118 J=2,IPPH
   DO 117 IK=1,IDL1
      CH2(IK,1) = CH2(IK,1)+CH2(IK,J)
  117    continue
  118 continue
  DO 120 J=2,IPPH
   JC = IPP2-J
   DO 119 IK=2,IDL1,2
      CH2(IK-1,J) = C2(IK-1,J)-C2(IK,JC)
      CH2(IK-1,JC) = C2(IK-1,J)+C2(IK,JC)
      CH2(IK,J) = C2(IK,J)+C2(IK-1,JC)
      CH2(IK,JC) = C2(IK,J)-C2(IK-1,JC)
  119    continue
  120 continue
  NAC = 1
  if (IDO == 2) return
  NAC = 0
  DO 121 IK=1,IDL1
   C2(IK,1) = CH2(IK,1)
  121 continue
  DO 123 J=2,IP
   DO 122 K=1,L1
      C1(1,K,J) = CH(1,K,J)
      C1(2,K,J) = CH(2,K,J)
  122    continue
  123 continue
  if (IDOT > L1) goto 127
  IDIJ = 0
  DO 126 J=2,IP
   IDIJ = IDIJ+2
   DO 125 I=4,IDO,2
      IDIJ = IDIJ+2
      DO 124 K=1,L1
         C1(I-1,K,J) = WA(IDIJ-1)*CH(I-1,K,J)-WA(IDIJ)*CH(I,K,J)
         C1(I,K,J) = WA(IDIJ-1)*CH(I,K,J)+WA(IDIJ)*CH(I-1,K,J)
  124       continue
  125    continue
  126 continue
  return
  127 IDJ = 2-IDO
  DO 130 J=2,IP
   IDJ = IDJ+IDO
   DO 129 K=1,L1
      IDIJ = IDJ
      DO 128 I=4,IDO,2
         IDIJ = IDIJ+2
         C1(I-1,K,J) = WA(IDIJ-1)*CH(I-1,K,J)-WA(IDIJ)*CH(I,K,J)
         C1(I,K,J) = WA(IDIJ-1)*CH(I,K,J)+WA(IDIJ)*CH(I-1,K,J)
  128       continue
  129    continue
  130 continue
  return
  END
  subroutine PASSB2 (IDO,L1,CC,CH,WA1)
  DIMENSION       CC(IDO,2,L1)           ,CH(IDO,L1,2), &
                  WA1(1)
  if (IDO > 2) goto 102
  DO 101 K=1,L1
   CH(1,K,1) = CC(1,1,K)+CC(1,2,K)
   CH(1,K,2) = CC(1,1,K)-CC(1,2,K)
   CH(2,K,1) = CC(2,1,K)+CC(2,2,K)
   CH(2,K,2) = CC(2,1,K)-CC(2,2,K)
  101 continue
  return
  102 DO 104 K=1,L1
   DO 103 I=2,IDO,2
      CH(I-1,K,1) = CC(I-1,1,K)+CC(I-1,2,K)
      TR2 = CC(I-1,1,K)-CC(I-1,2,K)
      CH(I,K,1) = CC(I,1,K)+CC(I,2,K)
      TI2 = CC(I,1,K)-CC(I,2,K)
      CH(I,K,2) = WA1(I-1)*TI2+WA1(I)*TR2
      CH(I-1,K,2) = WA1(I-1)*TR2-WA1(I)*TI2
  103    continue
  104 continue
  return
  END
  subroutine PASSB3 (IDO,L1,CC,CH,WA1,WA2)
  DIMENSION       CC(IDO,3,L1)           ,CH(IDO,L1,3), &
                  WA1(1)     ,WA2(1)
  DATA TAUR,TAUI /-.5,.866025403784439/
  if (IDO /= 2) goto 102
  DO 101 K=1,L1
   TR2 = CC(1,2,K)+CC(1,3,K)
   CR2 = CC(1,1,K)+TAUR*TR2
   CH(1,K,1) = CC(1,1,K)+TR2
   TI2 = CC(2,2,K)+CC(2,3,K)
   CI2 = CC(2,1,K)+TAUR*TI2
   CH(2,K,1) = CC(2,1,K)+TI2
   CR3 = TAUI*(CC(1,2,K)-CC(1,3,K))
   CI3 = TAUI*(CC(2,2,K)-CC(2,3,K))
   CH(1,K,2) = CR2-CI3
   CH(1,K,3) = CR2+CI3
   CH(2,K,2) = CI2+CR3
   CH(2,K,3) = CI2-CR3
  101 continue
  return
  102 DO 104 K=1,L1
   DO 103 I=2,IDO,2
      TR2 = CC(I-1,2,K)+CC(I-1,3,K)
      CR2 = CC(I-1,1,K)+TAUR*TR2
      CH(I-1,K,1) = CC(I-1,1,K)+TR2
      TI2 = CC(I,2,K)+CC(I,3,K)
      CI2 = CC(I,1,K)+TAUR*TI2
      CH(I,K,1) = CC(I,1,K)+TI2
      CR3 = TAUI*(CC(I-1,2,K)-CC(I-1,3,K))
      CI3 = TAUI*(CC(I,2,K)-CC(I,3,K))
      DR2 = CR2-CI3
      DR3 = CR2+CI3
      DI2 = CI2+CR3
      DI3 = CI2-CR3
      CH(I,K,2) = WA1(I-1)*DI2+WA1(I)*DR2
      CH(I-1,K,2) = WA1(I-1)*DR2-WA1(I)*DI2
      CH(I,K,3) = WA2(I-1)*DI3+WA2(I)*DR3
      CH(I-1,K,3) = WA2(I-1)*DR3-WA2(I)*DI3
  103    continue
  104 continue
  return
  END
  subroutine PASSB4 (IDO,L1,CC,CH,WA1,WA2,WA3)
  DIMENSION       CC(IDO,4,L1)           ,CH(IDO,L1,4), &
                  WA1(1)     ,WA2(1)     ,WA3(1)
  if (IDO /= 2) goto 102
  DO 101 K=1,L1
   TI1 = CC(2,1,K)-CC(2,3,K)
   TI2 = CC(2,1,K)+CC(2,3,K)
   TR4 = CC(2,4,K)-CC(2,2,K)
   TI3 = CC(2,2,K)+CC(2,4,K)
   TR1 = CC(1,1,K)-CC(1,3,K)
   TR2 = CC(1,1,K)+CC(1,3,K)
   TI4 = CC(1,2,K)-CC(1,4,K)
   TR3 = CC(1,2,K)+CC(1,4,K)
   CH(1,K,1) = TR2+TR3
   CH(1,K,3) = TR2-TR3
   CH(2,K,1) = TI2+TI3
   CH(2,K,3) = TI2-TI3
   CH(1,K,2) = TR1+TR4
   CH(1,K,4) = TR1-TR4
   CH(2,K,2) = TI1+TI4
   CH(2,K,4) = TI1-TI4
  101 continue
  return
  102 DO 104 K=1,L1
   DO 103 I=2,IDO,2
      TI1 = CC(I,1,K)-CC(I,3,K)
      TI2 = CC(I,1,K)+CC(I,3,K)
      TI3 = CC(I,2,K)+CC(I,4,K)
      TR4 = CC(I,4,K)-CC(I,2,K)
      TR1 = CC(I-1,1,K)-CC(I-1,3,K)
      TR2 = CC(I-1,1,K)+CC(I-1,3,K)
      TI4 = CC(I-1,2,K)-CC(I-1,4,K)
      TR3 = CC(I-1,2,K)+CC(I-1,4,K)
      CH(I-1,K,1) = TR2+TR3
      CR3 = TR2-TR3
      CH(I,K,1) = TI2+TI3
      CI3 = TI2-TI3
      CR2 = TR1+TR4
      CR4 = TR1-TR4
      CI2 = TI1+TI4
      CI4 = TI1-TI4
      CH(I-1,K,2) = WA1(I-1)*CR2-WA1(I)*CI2
      CH(I,K,2) = WA1(I-1)*CI2+WA1(I)*CR2
      CH(I-1,K,3) = WA2(I-1)*CR3-WA2(I)*CI3
      CH(I,K,3) = WA2(I-1)*CI3+WA2(I)*CR3
      CH(I-1,K,4) = WA3(I-1)*CR4-WA3(I)*CI4
      CH(I,K,4) = WA3(I-1)*CI4+WA3(I)*CR4
  103    continue
  104 continue
  return
  END
  subroutine PASSB5 (IDO,L1,CC,CH,WA1,WA2,WA3,WA4)
  DIMENSION       CC(IDO,5,L1)           ,CH(IDO,L1,5), &
                  WA1(1)     ,WA2(1)     ,WA3(1)     ,WA4(1)
  DATA TR11,TI11,TR12,TI12 /.309016994374947,.951056516295154, &
  -.809016994374947,.587785252292473/
  if (IDO /= 2) goto 102
  DO 101 K=1,L1
   TI5 = CC(2,2,K)-CC(2,5,K)
   TI2 = CC(2,2,K)+CC(2,5,K)
   TI4 = CC(2,3,K)-CC(2,4,K)
   TI3 = CC(2,3,K)+CC(2,4,K)
   TR5 = CC(1,2,K)-CC(1,5,K)
   TR2 = CC(1,2,K)+CC(1,5,K)
   TR4 = CC(1,3,K)-CC(1,4,K)
   TR3 = CC(1,3,K)+CC(1,4,K)
   CH(1,K,1) = CC(1,1,K)+TR2+TR3
   CH(2,K,1) = CC(2,1,K)+TI2+TI3
   CR2 = CC(1,1,K)+TR11*TR2+TR12*TR3
   CI2 = CC(2,1,K)+TR11*TI2+TR12*TI3
   CR3 = CC(1,1,K)+TR12*TR2+TR11*TR3
   CI3 = CC(2,1,K)+TR12*TI2+TR11*TI3
   CR5 = TI11*TR5+TI12*TR4
   CI5 = TI11*TI5+TI12*TI4
   CR4 = TI12*TR5-TI11*TR4
   CI4 = TI12*TI5-TI11*TI4
   CH(1,K,2) = CR2-CI5
   CH(1,K,5) = CR2+CI5
   CH(2,K,2) = CI2+CR5
   CH(2,K,3) = CI3+CR4
   CH(1,K,3) = CR3-CI4
   CH(1,K,4) = CR3+CI4
   CH(2,K,4) = CI3-CR4
   CH(2,K,5) = CI2-CR5
  101 continue
  return
  102 DO 104 K=1,L1
   DO 103 I=2,IDO,2
      TI5 = CC(I,2,K)-CC(I,5,K)
      TI2 = CC(I,2,K)+CC(I,5,K)
      TI4 = CC(I,3,K)-CC(I,4,K)
      TI3 = CC(I,3,K)+CC(I,4,K)
      TR5 = CC(I-1,2,K)-CC(I-1,5,K)
      TR2 = CC(I-1,2,K)+CC(I-1,5,K)
      TR4 = CC(I-1,3,K)-CC(I-1,4,K)
      TR3 = CC(I-1,3,K)+CC(I-1,4,K)
      CH(I-1,K,1) = CC(I-1,1,K)+TR2+TR3
      CH(I,K,1) = CC(I,1,K)+TI2+TI3
      CR2 = CC(I-1,1,K)+TR11*TR2+TR12*TR3
      CI2 = CC(I,1,K)+TR11*TI2+TR12*TI3
      CR3 = CC(I-1,1,K)+TR12*TR2+TR11*TR3
      CI3 = CC(I,1,K)+TR12*TI2+TR11*TI3
      CR5 = TI11*TR5+TI12*TR4
      CI5 = TI11*TI5+TI12*TI4
      CR4 = TI12*TR5-TI11*TR4
      CI4 = TI12*TI5-TI11*TI4
      DR3 = CR3-CI4
      DR4 = CR3+CI4
      DI3 = CI3+CR4
      DI4 = CI3-CR4
      DR5 = CR2+CI5
      DR2 = CR2-CI5
      DI5 = CI2-CR5
      DI2 = CI2+CR5
      CH(I-1,K,2) = WA1(I-1)*DR2-WA1(I)*DI2
      CH(I,K,2) = WA1(I-1)*DI2+WA1(I)*DR2
      CH(I-1,K,3) = WA2(I-1)*DR3-WA2(I)*DI3
      CH(I,K,3) = WA2(I-1)*DI3+WA2(I)*DR3
      CH(I-1,K,4) = WA3(I-1)*DR4-WA3(I)*DI4
      CH(I,K,4) = WA3(I-1)*DI4+WA3(I)*DR4
      CH(I-1,K,5) = WA4(I-1)*DR5-WA4(I)*DI5
      CH(I,K,5) = WA4(I-1)*DI5+WA4(I)*DR5
  103    continue
  104 continue
  return
  END

  subroutine CFFTI (N,WSAVE)
  DIMENSION       WSAVE(1)
  if (N == 1) return
  IW1 = N+N+1
  IW2 = IW1+N+N
  CALL CFFTI1 (N,WSAVE(IW1),WSAVE(IW2))
  return
  END
  subroutine CFFTI1 (N,WA,IFAC)
  DIMENSION       WA(1)      ,IFAC(1)    ,NTRYH(4)
  DATA NTRYH(1),NTRYH(2),NTRYH(3),NTRYH(4)/3,4,2,5/
  NL = N
  NF = 0
  J = 0
  101 J = J+1
  if (J-4) 102,102,103
  102 NTRY = NTRYH(J)
  goto 104
  103 NTRY = NTRY+2
  104 NQ = NL/NTRY
  NR = NL-NTRY*NQ
  if (NR) 101,105,101
  105 NF = NF+1
  IFAC(NF+2) = NTRY
  NL = NQ
  if (NTRY /= 2) goto 107
  if (NF == 1) goto 107
  DO 106 I=2,NF
   IB = NF-I+2
   IFAC(IB+2) = IFAC(IB+1)
  106 continue
  IFAC(3) = 2
  107 if (NL /= 1) goto 104
  IFAC(1) = N
  IFAC(2) = NF
  TPI = 6.28318530717959
  ARGH = TPI/FLOAT(N)
  I = 2
  L1 = 1
  DO 110 K1=1,NF
   IP = IFAC(K1+2)
   LD = 0
   L2 = L1*IP
   IDO = N/L2
   IDOT = IDO+IDO+2
   IPM = IP-1
   DO 109 J=1,IPM
      I1 = I
      WA(I-1) = 1.
      WA(I) = 0.
      LD = LD+L1
      FI = 0.
      ARGLD = FLOAT(LD)*ARGH
      DO 108 II=4,IDOT,2
         I = I+2
         FI = FI+1.
         ARG = FI*ARGLD
         WA(I-1) = COS(ARG)
         WA(I) = SIN(ARG)
  108       continue
      if (IP <= 5) goto 109
      WA(I1-1) = WA(I-1)
      WA(I1) = WA(I)
  109    continue
   L1 = L2
  110 continue
  return
  END

  subroutine CFFTF (N,C,WSAVE)
  DIMENSION       C(1)       ,WSAVE(1)
  if (N == 1) return
  IW1 = N+N+1
  IW2 = IW1+N+N
  CALL CFFTF1 (N,C,WSAVE,WSAVE(IW1),WSAVE(IW2))
  return
  END
  subroutine CFFTF1 (N,C,CH,WA,IFAC)
  DIMENSION       CH(1)      ,C(1)       ,WA(1)      ,IFAC(1)
  NF = IFAC(2)
  NA = 0
  L1 = 1
  IW = 1
  DO 116 K1=1,NF
   IP = IFAC(K1+2)
   L2 = IP*L1
   IDO = N/L2
   IDOT = IDO+IDO
   IDL1 = IDOT*L1
   if (IP /= 4) goto 103
   IX2 = IW+IDOT
   IX3 = IX2+IDOT
   if (NA /= 0) goto 101
   CALL PASSF4 (IDOT,L1,C,CH,WA(IW),WA(IX2),WA(IX3))
   goto 102
  101    CALL PASSF4 (IDOT,L1,CH,C,WA(IW),WA(IX2),WA(IX3))
  102    NA = 1-NA
   goto 115
  103    if (IP /= 2) goto 106
   if (NA /= 0) goto 104
   CALL PASSF2 (IDOT,L1,C,CH,WA(IW))
   goto 105
  104    CALL PASSF2 (IDOT,L1,CH,C,WA(IW))
  105    NA = 1-NA
   goto 115
  106    if (IP /= 3) goto 109
   IX2 = IW+IDOT
   if (NA /= 0) goto 107
   CALL PASSF3 (IDOT,L1,C,CH,WA(IW),WA(IX2))
   goto 108
  107    CALL PASSF3 (IDOT,L1,CH,C,WA(IW),WA(IX2))
  108    NA = 1-NA
   goto 115
  109    if (IP /= 5) goto 112
   IX2 = IW+IDOT
   IX3 = IX2+IDOT
   IX4 = IX3+IDOT
   if (NA /= 0) goto 110
   CALL PASSF5 (IDOT,L1,C,CH,WA(IW),WA(IX2),WA(IX3),WA(IX4))
   goto 111
  110    CALL PASSF5 (IDOT,L1,CH,C,WA(IW),WA(IX2),WA(IX3),WA(IX4))
  111    NA = 1-NA
   goto 115
  112    if (NA /= 0) goto 113
   CALL PASSF (NAC,IDOT,IP,L1,IDL1,C,C,C,CH,CH,WA(IW))
   goto 114
  113    CALL PASSF (NAC,IDOT,IP,L1,IDL1,CH,CH,CH,C,C,WA(IW))
  114    if (NAC /= 0) NA = 1-NA
  115    L1 = L2
   IW = IW+(IP-1)*IDOT
  116 continue
  if (NA == 0) return
  N2 = N+N
  DO 117 I=1,N2
   C(I) = CH(I)
  117 continue
  return
  END
  subroutine PASSF (NAC,IDO,IP,L1,IDL1,CC,C1,C2,CH,CH2,WA)
  DIMENSION       CH(IDO,L1,IP)          ,CC(IDO,IP,L1), &
                  C1(IDO,L1,IP)          ,WA(1)      ,C2(IDL1,IP), &
                  CH2(IDL1,IP)
  IDOT = IDO/2
  NT = IP*IDL1
  IPP2 = IP+2
  IPPH = (IP+1)/2
  IDP = IP*IDO
!
  if (IDO < L1) goto 106
  DO 103 J=2,IPPH
   JC = IPP2-J
   DO 102 K=1,L1
      DO 101 I=1,IDO
         CH(I,K,J) = CC(I,J,K)+CC(I,JC,K)
         CH(I,K,JC) = CC(I,J,K)-CC(I,JC,K)
  101       continue
  102    continue
  103 continue
  DO 105 K=1,L1
   DO 104 I=1,IDO
      CH(I,K,1) = CC(I,1,K)
  104    continue
  105 continue
  goto 112
  106 DO 109 J=2,IPPH
   JC = IPP2-J
   DO 108 I=1,IDO
      DO 107 K=1,L1
         CH(I,K,J) = CC(I,J,K)+CC(I,JC,K)
         CH(I,K,JC) = CC(I,J,K)-CC(I,JC,K)
  107       continue
  108    continue
  109 continue
  DO 111 I=1,IDO
   DO 110 K=1,L1
      CH(I,K,1) = CC(I,1,K)
  110    continue
  111 continue
  112 IDL = 2-IDO
  INC = 0
  DO 116 L=2,IPPH
   LC = IPP2-L
   IDL = IDL+IDO
   DO 113 IK=1,IDL1
      C2(IK,L) = CH2(IK,1)+WA(IDL-1)*CH2(IK,2)
      C2(IK,LC) = -WA(IDL)*CH2(IK,IP)
  113    continue
   IDLJ = IDL
   INC = INC+IDO
   DO 115 J=3,IPPH
      JC = IPP2-J
      IDLJ = IDLJ+INC
      if (IDLJ > IDP) IDLJ = IDLJ-IDP
      WAR = WA(IDLJ-1)
      WAI = WA(IDLJ)
      DO 114 IK=1,IDL1
         C2(IK,L) = C2(IK,L)+WAR*CH2(IK,J)
         C2(IK,LC) = C2(IK,LC)-WAI*CH2(IK,JC)
  114       continue
  115    continue
  116 continue
  DO 118 J=2,IPPH
   DO 117 IK=1,IDL1
      CH2(IK,1) = CH2(IK,1)+CH2(IK,J)
  117    continue
  118 continue
  DO 120 J=2,IPPH
   JC = IPP2-J
   DO 119 IK=2,IDL1,2
      CH2(IK-1,J) = C2(IK-1,J)-C2(IK,JC)
      CH2(IK-1,JC) = C2(IK-1,J)+C2(IK,JC)
      CH2(IK,J) = C2(IK,J)+C2(IK-1,JC)
      CH2(IK,JC) = C2(IK,J)-C2(IK-1,JC)
  119    continue
  120 continue
  NAC = 1
  if (IDO == 2) return
  NAC = 0
  DO 121 IK=1,IDL1
   C2(IK,1) = CH2(IK,1)
  121 continue
  DO 123 J=2,IP
   DO 122 K=1,L1
      C1(1,K,J) = CH(1,K,J)
      C1(2,K,J) = CH(2,K,J)
  122    continue
  123 continue
  if (IDOT > L1) goto 127
  IDIJ = 0
  DO 126 J=2,IP
   IDIJ = IDIJ+2
   DO 125 I=4,IDO,2
      IDIJ = IDIJ+2
      DO 124 K=1,L1
         C1(I-1,K,J) = WA(IDIJ-1)*CH(I-1,K,J)+WA(IDIJ)*CH(I,K,J)
         C1(I,K,J) = WA(IDIJ-1)*CH(I,K,J)-WA(IDIJ)*CH(I-1,K,J)
  124       continue
  125    continue
  126 continue
  return
  127 IDJ = 2-IDO
  DO 130 J=2,IP
   IDJ = IDJ+IDO
   DO 129 K=1,L1
      IDIJ = IDJ
      DO 128 I=4,IDO,2
         IDIJ = IDIJ+2
         C1(I-1,K,J) = WA(IDIJ-1)*CH(I-1,K,J)+WA(IDIJ)*CH(I,K,J)
         C1(I,K,J) = WA(IDIJ-1)*CH(I,K,J)-WA(IDIJ)*CH(I-1,K,J)
  128       continue
  129    continue
  130 continue
  return
  END
  subroutine PASSF2 (IDO,L1,CC,CH,WA1)
  DIMENSION       CC(IDO,2,L1)           ,CH(IDO,L1,2), &
                  WA1(1)
  if (IDO > 2) goto 102
  DO 101 K=1,L1
   CH(1,K,1) = CC(1,1,K)+CC(1,2,K)
   CH(1,K,2) = CC(1,1,K)-CC(1,2,K)
   CH(2,K,1) = CC(2,1,K)+CC(2,2,K)
   CH(2,K,2) = CC(2,1,K)-CC(2,2,K)
  101 continue
  return
  102 DO 104 K=1,L1
   DO 103 I=2,IDO,2
      CH(I-1,K,1) = CC(I-1,1,K)+CC(I-1,2,K)
      TR2 = CC(I-1,1,K)-CC(I-1,2,K)
      CH(I,K,1) = CC(I,1,K)+CC(I,2,K)
      TI2 = CC(I,1,K)-CC(I,2,K)
      CH(I,K,2) = WA1(I-1)*TI2-WA1(I)*TR2
      CH(I-1,K,2) = WA1(I-1)*TR2+WA1(I)*TI2
  103    continue
  104 continue
  return
  END
  subroutine PASSF3 (IDO,L1,CC,CH,WA1,WA2)
  DIMENSION       CC(IDO,3,L1)           ,CH(IDO,L1,3), &
                  WA1(1)     ,WA2(1)
  DATA TAUR,TAUI /-.5,-.866025403784439/
  if (IDO /= 2) goto 102
  DO 101 K=1,L1
   TR2 = CC(1,2,K)+CC(1,3,K)
   CR2 = CC(1,1,K)+TAUR*TR2
   CH(1,K,1) = CC(1,1,K)+TR2
   TI2 = CC(2,2,K)+CC(2,3,K)
   CI2 = CC(2,1,K)+TAUR*TI2
   CH(2,K,1) = CC(2,1,K)+TI2
   CR3 = TAUI*(CC(1,2,K)-CC(1,3,K))
   CI3 = TAUI*(CC(2,2,K)-CC(2,3,K))
   CH(1,K,2) = CR2-CI3
   CH(1,K,3) = CR2+CI3
   CH(2,K,2) = CI2+CR3
   CH(2,K,3) = CI2-CR3
  101 continue
  return
  102 DO 104 K=1,L1
   DO 103 I=2,IDO,2
      TR2 = CC(I-1,2,K)+CC(I-1,3,K)
      CR2 = CC(I-1,1,K)+TAUR*TR2
      CH(I-1,K,1) = CC(I-1,1,K)+TR2
      TI2 = CC(I,2,K)+CC(I,3,K)
      CI2 = CC(I,1,K)+TAUR*TI2
      CH(I,K,1) = CC(I,1,K)+TI2
      CR3 = TAUI*(CC(I-1,2,K)-CC(I-1,3,K))
      CI3 = TAUI*(CC(I,2,K)-CC(I,3,K))
      DR2 = CR2-CI3
      DR3 = CR2+CI3
      DI2 = CI2+CR3
      DI3 = CI2-CR3
      CH(I,K,2) = WA1(I-1)*DI2-WA1(I)*DR2
      CH(I-1,K,2) = WA1(I-1)*DR2+WA1(I)*DI2
      CH(I,K,3) = WA2(I-1)*DI3-WA2(I)*DR3
      CH(I-1,K,3) = WA2(I-1)*DR3+WA2(I)*DI3
  103    continue
  104 continue
  return
  END
  subroutine PASSF4 (IDO,L1,CC,CH,WA1,WA2,WA3)
  DIMENSION       CC(IDO,4,L1)           ,CH(IDO,L1,4), &
                  WA1(1)     ,WA2(1)     ,WA3(1)
  if (IDO /= 2) goto 102
  DO 101 K=1,L1
   TI1 = CC(2,1,K)-CC(2,3,K)
   TI2 = CC(2,1,K)+CC(2,3,K)
   TR4 = CC(2,2,K)-CC(2,4,K)
   TI3 = CC(2,2,K)+CC(2,4,K)
   TR1 = CC(1,1,K)-CC(1,3,K)
   TR2 = CC(1,1,K)+CC(1,3,K)
   TI4 = CC(1,4,K)-CC(1,2,K)
   TR3 = CC(1,2,K)+CC(1,4,K)
   CH(1,K,1) = TR2+TR3
   CH(1,K,3) = TR2-TR3
   CH(2,K,1) = TI2+TI3
   CH(2,K,3) = TI2-TI3
   CH(1,K,2) = TR1+TR4
   CH(1,K,4) = TR1-TR4
   CH(2,K,2) = TI1+TI4
   CH(2,K,4) = TI1-TI4
  101 continue
  return
  102 DO 104 K=1,L1
   DO 103 I=2,IDO,2
      TI1 = CC(I,1,K)-CC(I,3,K)
      TI2 = CC(I,1,K)+CC(I,3,K)
      TI3 = CC(I,2,K)+CC(I,4,K)
      TR4 = CC(I,2,K)-CC(I,4,K)
      TR1 = CC(I-1,1,K)-CC(I-1,3,K)
      TR2 = CC(I-1,1,K)+CC(I-1,3,K)
      TI4 = CC(I-1,4,K)-CC(I-1,2,K)
      TR3 = CC(I-1,2,K)+CC(I-1,4,K)
      CH(I-1,K,1) = TR2+TR3
      CR3 = TR2-TR3
      CH(I,K,1) = TI2+TI3
      CI3 = TI2-TI3
      CR2 = TR1+TR4
      CR4 = TR1-TR4
      CI2 = TI1+TI4
      CI4 = TI1-TI4
      CH(I-1,K,2) = WA1(I-1)*CR2+WA1(I)*CI2
      CH(I,K,2) = WA1(I-1)*CI2-WA1(I)*CR2
      CH(I-1,K,3) = WA2(I-1)*CR3+WA2(I)*CI3
      CH(I,K,3) = WA2(I-1)*CI3-WA2(I)*CR3
      CH(I-1,K,4) = WA3(I-1)*CR4+WA3(I)*CI4
      CH(I,K,4) = WA3(I-1)*CI4-WA3(I)*CR4
  103    continue
  104 continue
  return
  END
  subroutine PASSF5 (IDO,L1,CC,CH,WA1,WA2,WA3,WA4)
  DIMENSION       CC(IDO,5,L1)           ,CH(IDO,L1,5), &
                  WA1(1)     ,WA2(1)     ,WA3(1)     ,WA4(1)
  DATA TR11,TI11,TR12,TI12 /.309016994374947,-.951056516295154, &
  -.809016994374947,-.587785252292473/
  if (IDO /= 2) goto 102
  DO 101 K=1,L1
   TI5 = CC(2,2,K)-CC(2,5,K)
   TI2 = CC(2,2,K)+CC(2,5,K)
   TI4 = CC(2,3,K)-CC(2,4,K)
   TI3 = CC(2,3,K)+CC(2,4,K)
   TR5 = CC(1,2,K)-CC(1,5,K)
   TR2 = CC(1,2,K)+CC(1,5,K)
   TR4 = CC(1,3,K)-CC(1,4,K)
   TR3 = CC(1,3,K)+CC(1,4,K)
   CH(1,K,1) = CC(1,1,K)+TR2+TR3
   CH(2,K,1) = CC(2,1,K)+TI2+TI3
   CR2 = CC(1,1,K)+TR11*TR2+TR12*TR3
   CI2 = CC(2,1,K)+TR11*TI2+TR12*TI3
   CR3 = CC(1,1,K)+TR12*TR2+TR11*TR3
   CI3 = CC(2,1,K)+TR12*TI2+TR11*TI3
   CR5 = TI11*TR5+TI12*TR4
   CI5 = TI11*TI5+TI12*TI4
   CR4 = TI12*TR5-TI11*TR4
   CI4 = TI12*TI5-TI11*TI4
   CH(1,K,2) = CR2-CI5
   CH(1,K,5) = CR2+CI5
   CH(2,K,2) = CI2+CR5
   CH(2,K,3) = CI3+CR4
   CH(1,K,3) = CR3-CI4
   CH(1,K,4) = CR3+CI4
   CH(2,K,4) = CI3-CR4
   CH(2,K,5) = CI2-CR5
  101 continue
  return
  102 DO 104 K=1,L1
   DO 103 I=2,IDO,2
      TI5 = CC(I,2,K)-CC(I,5,K)
      TI2 = CC(I,2,K)+CC(I,5,K)
      TI4 = CC(I,3,K)-CC(I,4,K)
      TI3 = CC(I,3,K)+CC(I,4,K)
      TR5 = CC(I-1,2,K)-CC(I-1,5,K)
      TR2 = CC(I-1,2,K)+CC(I-1,5,K)
      TR4 = CC(I-1,3,K)-CC(I-1,4,K)
      TR3 = CC(I-1,3,K)+CC(I-1,4,K)
      CH(I-1,K,1) = CC(I-1,1,K)+TR2+TR3
      CH(I,K,1) = CC(I,1,K)+TI2+TI3
      CR2 = CC(I-1,1,K)+TR11*TR2+TR12*TR3
      CI2 = CC(I,1,K)+TR11*TI2+TR12*TI3
      CR3 = CC(I-1,1,K)+TR12*TR2+TR11*TR3
      CI3 = CC(I,1,K)+TR12*TI2+TR11*TI3
      CR5 = TI11*TR5+TI12*TR4
      CI5 = TI11*TI5+TI12*TI4
      CR4 = TI12*TR5-TI11*TR4
      CI4 = TI12*TI5-TI11*TI4
      DR3 = CR3-CI4
      DR4 = CR3+CI4
      DI3 = CI3+CR4
      DI4 = CI3-CR4
      DR5 = CR2+CI5
      DR2 = CR2-CI5
      DI5 = CI2-CR5
      DI2 = CI2+CR5
      CH(I-1,K,2) = WA1(I-1)*DR2+WA1(I)*DI2
      CH(I,K,2) = WA1(I-1)*DI2-WA1(I)*DR2
      CH(I-1,K,3) = WA2(I-1)*DR3+WA2(I)*DI3
      CH(I,K,3) = WA2(I-1)*DI3-WA2(I)*DR3
      CH(I-1,K,4) = WA3(I-1)*DR4+WA3(I)*DI4
      CH(I,K,4) = WA3(I-1)*DI4-WA3(I)*DR4
      CH(I-1,K,5) = WA4(I-1)*DR5+WA4(I)*DI5
      CH(I,K,5) = WA4(I-1)*DI5-WA4(I)*DR5
  103    continue
  104 continue
  return
  END


!-------------------------------------------------------------------------------
! Copyright (c) 2016 The University of Tokyo
! This software is released under the MIT License, see LICENSE.txt
!-------------------------------------------------------------------------------
!> \brief  This module contains functions for interpolation in 9 node
!!   quadrilateral element
module shape_quad9n
  !####################################################################

  implicit none

  !--------------------------------------------------------------------

  integer, parameter, private :: kreal = kind( 0.0D0 )

  !--------------------------------------------------------------------

contains

  !####################################################################
  subroutine ShapeFunc_quad9n(lcoord, func)
    !####################################################################

    real(kind = kreal), intent(in)  :: lcoord(2)
    real(kind = kreal), intent(out) :: func(9)

    !--------------------------------------------------------------------

    real(kind = kreal) :: xi(9), eta(9)
    real(kind = kreal) :: n_xi(9), n_eta(9)

    integer :: na

    !--------------------------------------------------------------------

    ! xi-coordinate at a node in a local element
    xi(1)  = -1.0D0
    xi(2)  =  1.0D0
    xi(3)  =  1.0D0
    xi(4)  = -1.0D0
    xi(5)  =  0.0D0
    xi(6)  =  1.0D0
    xi(7)  =  0.0D0
    xi(8)  = -1.0D0
    xi(9)  =  0.0D0
    ! eta-coordinate at a node in a local element
    eta(1) = -1.0D0
    eta(2) = -1.0D0
    eta(3) =  1.0D0
    eta(4) =  1.0D0
    eta(5) = -1.0D0
    eta(6) =  0.0D0
    eta(7) =  1.0D0
    eta(8) =  0.0D0
    eta(9) =  0.0D0

    !--------------------------------------------------------------------

    do na = 1, 9

      n_xi(na)  = ( 0.5D0*xi(na) *lcoord(1) )    &
        *( 1.0D0+xi(na) *lcoord(1) )   &
        +( 1.0D0-xi(na) *xi(na)  )      &
        *( 1.0D0-lcoord(1)*lcoord(1) )
      n_eta(na) = ( 0.5D0*eta(na)*lcoord(2) )    &
        *( 1.0D0+eta(na)*lcoord(2) )   &
        +( 1.0D0-eta(na)*eta(na) )      &
        *( 1.0D0-lcoord(2)*lcoord(2) )

    end do

    !--------------------------------------------------------------------

    do na = 1, 9

      func(na) = n_xi(na)*n_eta(na)

    end do

    !--------------------------------------------------------------------

    return

    !####################################################################
  end subroutine ShapeFunc_quad9n
  !####################################################################

  !####################################################################
  subroutine ShapeDeriv_quad9n(lcoord, func)
    !####################################################################

    real(kind = kreal), intent(in)  :: lcoord(2)
    real(kind = kreal), intent(out) :: func(9, 2)

    !--------------------------------------------------------------------

    real(kind = kreal) :: xi(9), eta(9)
    real(kind = kreal) :: n_xi(9), n_eta(9)
    real(kind = kreal) :: dn_xi(9), dn_eta(9)

    integer :: na

    !--------------------------------------------------------------------

    ! xi-coordinate at a node in a local element
    xi(1)  = -1.0D0
    xi(2)  =  1.0D0
    xi(3)  =  1.0D0
    xi(4)  = -1.0D0
    xi(5)  =  0.0D0
    xi(6)  =  1.0D0
    xi(7)  =  0.0D0
    xi(8)  = -1.0D0
    xi(9)  =  0.0D0
    ! eta-coordinate at a node in a local element
    eta(1) = -1.0D0
    eta(2) = -1.0D0
    eta(3) =  1.0D0
    eta(4) =  1.0D0
    eta(5) = -1.0D0
    eta(6) =  0.0D0
    eta(7) =  1.0D0
    eta(8) =  0.0D0
    eta(9) =  0.0D0

    !--------------------------------------------------------------------

    do na = 1, 9

      n_xi(na)  = ( 0.5D0*xi(na) *lcoord(1) )    &
        *( 1.0D0+xi(na) *lcoord(1) )   &
        +( 1.0D0-xi(na) *xi(na)  )      &
        *( 1.0D0-lcoord(1)*lcoord(1) )
      n_eta(na) = ( 0.5D0*eta(na)*lcoord(2) )    &
        *( 1.0D0+eta(na)*lcoord(2) )   &
        +( 1.0D0-eta(na)*eta(na) )      &
        *( 1.0D0-lcoord(2)*lcoord(2) )

      dn_xi(na)  = ( 0.5D0*xi(na)  )            &
        *( 1.0D0+xi(na) *lcoord(1) ) &
        +( 0.5D0*xi(na) *lcoord(1) )  &
        *xi(na)                      &
        +( 1.0D0-xi(na) *xi(na)  )    &
        *( -2.0D0*lcoord(1) )
      dn_eta(na) = ( 0.5D0*eta(na) )            &
        *( 1.0D0+eta(na)*lcoord(2) ) &
        +( 0.5D0*eta(na)*lcoord(2) )  &
        *eta(na)                     &
        +( 1.0D0-eta(na)*eta(na) )    &
        *( -2.0D0*lcoord(2) )

    end do

    !--------------------------------------------------------------------

    do na = 1, 9

      func(na, 1) = dn_xi(na)*n_eta(na)
      func(na, 2) = n_xi(na) *dn_eta(na)

    end do

    !--------------------------------------------------------------------

    return

    !####################################################################
  end subroutine ShapeDeriv_quad9n
  !####################################################################

  !####################################################################
  subroutine NodalNaturalCoord_quad9n(nncoord)
    !####################################################################

    real(kind = kreal), intent(out) :: nncoord(9, 2)

    !--------------------------------------------------------------------

    ! xi-coordinate at a node in a local element
    nncoord(1, 1)  = -1.0D0
    nncoord(2, 1)  =  1.0D0
    nncoord(3, 1)  =  1.0D0
    nncoord(4, 1)  = -1.0D0
    nncoord(5, 1)  =  0.0D0
    nncoord(6, 1)  =  1.0D0
    nncoord(7, 1)  =  0.0D0
    nncoord(8, 1)  = -1.0D0
    nncoord(9, 1)  =  0.0D0
    ! eta-coordinate at a node in a local element
    nncoord(1, 2) = -1.0D0
    nncoord(2, 2) = -1.0D0
    nncoord(3, 2) =  1.0D0
    nncoord(4, 2) =  1.0D0
    nncoord(5, 2) = -1.0D0
    nncoord(6, 2) =  0.0D0
    nncoord(7, 2) =  1.0D0
    nncoord(8, 2) =  0.0D0
    nncoord(9, 2) =  0.0D0

    !--------------------------------------------------------------------

    return

    !####################################################################
  end subroutine NodalNaturalCoord_quad9n
  !####################################################################

end module shape_quad9n


program sail_gen
!-------------------------------------------------------!
!	PROGRAM TO GENERATE DARPA SUBOFF SAIL		!
!	FULL/MODEL RATIO = 24				!
!	POINTS CALCULATED BY EQUATIONS:	UNIT = FEET	!
! SAIL FOREBODY EQ.  FOR   3.032986 <= X <= 3.358507	!
!			   0.833333 <= Z <= 1.507813	!
! SAIL MID-BODY EQ.  FOR   3.358507 <= X <= 3.559028	!
!			   0.833333 <= Z <= 1.507813    !
! SAIL AFTERBODY EQ. FOR   3.559028 <= X <= 4.241319	!
!			   0.833333 <= Z <= 1.507813    !
! SAIL CAP EQ.	     FOR   3.032986 <= X <= 4.241319	!
!			   1.507813 <= Z <= 1.562501    !
! 	OFFSETS ARE COMPUTED EVERY 0.005 FT		!
!							!
!		Xiaolong Tang: tangxl@buaa.edu.cn	!
!				       01/26/2018	!
!		        	BEIHANG UNIVERSITY	!
!-------------------------------------------------------!
IMPLICIT NONE
REAL(8) A1,B1,A3,B3,C3,D3,E3,HMAX,DX,DX0,XXCST,XXAFN,XXFFN,XXMFN,XZST,XZEND
REAL(8) XZ,XX,ADUM
REAL(8) X,DM1,A,B,C,D,E,F,G,H,P
INTEGER I,J,NI,NP,ICON1,N_OMIT
DIMENSION NP(300),X(300,50,3)
CHARACTER(LEN=20) FILENAME
!-------------------------------!
!	DEFINE CONSTANTS	!
!-------------------------------!
A1    =  2.094759
B1    =  0.207178
A3    =  2.908891
B3    =  1.234491
C3    =  3.444817
D3    =  3.850435
E3    =  2.080019
HMAX  =  0.109375
!MAX HALF THICKNESS OF SAIL
DX    =  0.005
DX0   =  0.005
XXCST =  3.032986
XXAFN =  4.241319
XXFFN =  3.358507
XXMFN =  3.559028
XZST  =  1.507813
XX=XXCST-DX
DO 1000 I=1,300
  XZ=XZST
  X(I,1,3)=XZ
  ! (I,1,3) = (POINT I, LAYER 1, AXIS 1/2/3)
  ! AXIS 1 = XX = LENGTH, 2 = XY = WIDTH, 3 = XZ = HEIGHT
  J=1
  XX=XX+DX
  X(I,1,1)=XX
  IF(XX.GT.XXAFN) THEN
  ! XX REACHED THE ENDPOINT
  ! NI = NUMBER OF EFFECTIVE POINTS
    NI=I-1
    GOTO 1014
  ENDIF
  IF(XX.GT.XXFFN) GOTO 1002
!-------------------------------!
!    SAIL FOREBODY EQUATION     !
!-------------------------------!
  D=3.072000*(XX-3.032986)
  DM1=D-1
  A=2*D*(DM1**4)
  B=D*D*(DM1**3)/3
  C=1- ((DM1**4)*(4*D+1))
  X(I,1,2)=HMAX*(SQRT(A1*A+B1*B+C))
  GOTO 1004
!-------------------------------!
!    SAIL MID-BODY EQUATION     !
!-------------------------------!
1002 CONTINUE
  IF(XX.GT.XXMFN) GOTO 1003
  X(I,1,2)=HMAX
  GOTO 1004
!-------------------------------!
!    SAIL AFTER-BODY EQUATION   !
!-------------------------------!
1003 CONTINUE
  E=(4.241319-XX)/0.6822917
  F=E-1
  G=2.238361*E*F**4
  H=3.106529*(E**2)*(F**3)
  P=1-(F**4)*(4*E+1)
  X(I,1,2)=0.1093750*(G+H+P)
!-------------------------------!
!      SAIL CAP EQUATION        !
!-------------------------------!
1004 CONTINUE
  XZEND=(X(I,1,2)/2)+1.507813
  NP(I)=1
  DO 1008 J=2,50
    ICON1=0
1005 XZ=XZ+DX
  X(I,J,3)=XZ
  IF(XZ.GT.XZEND) THEN
    ICON1=ICON1+1
    IF(ICON1.EQ.1) THEN
      XZ=XZ-DX
      DX=0.0005
      GOTO 1005
    ENDIF
    IF(ICON1.EQ.2) THEN
      X(I,J,2)=0.0
      X(I,J,3)=XZEND
      NP(I)=J
      ICON1=0
      DX=DX0
      GOTO 1000
    ENDIF
  ENDIF
  ADUM=(X(I,1,2)**2)-((2*(XZ-XZST))**2)
  X(I,J,2)=SQRT(ADUM)
1008 CONTINUE
1000 CONTINUE
!-------------------------------!
!    WRITE OFFSETS TO FILE      !
!-------------------------------!
1014 OPEN (10,FILE="sail-parallel-top.dat",STATUS="UNKNOWN")
WRITE(10,*) NI,1
DO I=1,NI
  WRITE(10,4) X(I,1,1),X(I,1,2),X(I,1,3)
ENDDO
CLOSE(10)
OPEN (20,FILE="sail-parallel-bottom.dat",STATUS="UNKNOWN")
WRITE(20,*) NI,1
DO I=1,NI
  WRITE(20,4) X(I,1,1),X(I,1,2),0.0
ENDDO
CLOSE(20)
OPEN (30,FILE="sail-cap.dat",STATUS="UNKNOWN")
! OMIT OUTPUT EVERY 8 STEPS
N_OMIT=8
DO 1013 I=1,NI,N_OMIT
    ! THE LINE BELOW IS FORMAT DATA FOR ICEMCFD INPUT
    WRITE(30,*) NP(I)+1,1
!  WRITE(30,'(I3)') I
!  WRITE(30,'(A2,F7.3,A4)') "X=",X(I,1,1),"FEET"
!  WRITE(30,'(I5)') NP(I)+1
  WRITE(30,4) X(I,1,1),X(I,1,2),1.507813
  DO J=1,NP(I)
    WRITE(30,4) X(I,1,1),X(I,J,2),X(I,J,3)
  ENDDO
4 FORMAT(3F16.12)
1013 CONTINUE
CLOSE(30)
STOP
END


program problem69
  implicit none
  integer, parameter    :: n=78498,limit=1000000
  integer, dimension(n) :: primes
  integer               :: i,temp

  ! https://primes.utm.edu/lists/small/100000.txt
  open(33,file='./data/problem69.dat')
  read(33,*) primes

  ! https://en.wikipedia.org/wiki/Euler%27s_totient_function
  ! $\frac{n}{\phi(n)} = \prod_{p|n}(\frac{p}{p-1})$
  ! Factors with powers greater than one increase n but not n/phi(n).
  ! Small factors are preferable to large ones, so greedily multiply.
  temp=1
  do i=1,n
     temp=temp*primes(i)
     if (temp>limit) then
        temp=temp/primes(i)
        go to 100
     end if
  end do

100 continue
  print *, temp

end program problem69


      subroutine spline (n, x, y, b, c, d)
      integer n
      real x(n), y(n), b(n), c(n), d(n)
c
c  the coefficients b(i), c(i), and d(i), i=1,2,...,n are computed
c  for a cubic interpolating spline
c
c    s(x) = y(i) + b(i)*(x-x(i)) + c(i)*(x-x(i))**2 + d(i)*(x-x(i))**3
c
c    for  x(i) .le. x .le. x(i+1)
c
c  input..
c
c    n = the number of data points or knots (n.ge.2)
c    x = the abscissas of the knots in strictly increasing order
c    y = the ordinates of the knots
c
c  output..
c
c    b, c, d  = arrays of spline coefficients as defined above.
c
c  using  p  to denote differentiation,
c
c    y(i) = s(x(i))
c    b(i) = sp(x(i))
c    c(i) = spp(x(i))/2
c    d(i) = sppp(x(i))/6  (derivative from the right)
c
c  the accompanying function subprogram  seval  can be used
c  to evaluate the spline.
c
c
      integer nm1, ib, i
      real t
c
      nm1 = n-1
      if ( n .lt. 2 ) return
      if ( n .lt. 3 ) go to 50
c
c  set up tridiagonal system
c
c  b = diagonal, d = offdiagonal, c = right hand side.
c
      d(1) = x(2) - x(1)
      c(2) = (y(2) - y(1))/d(1)
      do 10 i = 2, nm1
         d(i) = x(i+1) - x(i)
         b(i) = 2.*(d(i-1) + d(i))
         c(i+1) = (y(i+1) - y(i))/d(i)
         c(i) = c(i+1) - c(i)
   10 continue
c
c  end conditions.  third derivatives at  x(1)  and  x(n)
c  obtained from divided differences
c
      b(1) = -d(1)
      b(n) = -d(n-1)
      c(1) = 0.
      c(n) = 0.
      if ( n .eq. 3 ) go to 15
      c(1) = c(3)/(x(4)-x(2)) - c(2)/(x(3)-x(1))
      c(n) = c(n-1)/(x(n)-x(n-2)) - c(n-2)/(x(n-1)-x(n-3))
      c(1) = c(1)*d(1)**2/(x(4)-x(1))
      c(n) = -c(n)*d(n-1)**2/(x(n)-x(n-3))
c
c  forward elimination
c
   15 do 20 i = 2, n
         t = d(i-1)/b(i-1)
         b(i) = b(i) - t*d(i-1)
         c(i) = c(i) - t*c(i-1)
   20 continue
c
c  back substitution
c
      c(n) = c(n)/b(n)
      do 30 ib = 1, nm1
         i = n-ib
         c(i) = (c(i) - d(i)*c(i+1))/b(i)
   30 continue
c
c  c(i) is now the sigma(i) of the text
c
c  compute polynomial coefficients
c
      b(n) = (y(n) - y(nm1))/d(nm1) + d(nm1)*(c(nm1) + 2.*c(n))
      do 40 i = 1, nm1
         b(i) = (y(i+1) - y(i))/d(i) - d(i)*(c(i+1) + 2.*c(i))
         d(i) = (c(i+1) - c(i))/d(i)
         c(i) = 3.*c(i)
   40 continue
      c(n) = 3.*c(n)
      d(n) = d(n-1)
      return
c
   50 b(1) = (y(2)-y(1))/(x(2)-x(1))
      c(1) = 0.
      d(1) = 0.
      b(2) = b(1)
      c(2) = 0.
      d(2) = 0.
      return
      end


	subroutine cor(ni,nj,psi0,psi,auto,au_x,au_y)
	implicit none
	integer*4 i,j,ni,nj
	real*8 :: xmin,ymin,xmax,ymax,dx,dy,x,y,wx,wy
	complex*16,intent(IN) :: psi0(ni,nj),psi(ni,nj)
	complex*16 :: im
	real*8,intent(out) :: auto
	real*8 :: qx0,qy0,ax,ay,au_x,au_y,px0,py0
      real*8 cor_d

	common /grid/ xmin,ymin,xmax,ymax,dx,dy
	common /para1/ wx,wy
      common /ini/ qx0,qy0,px0,py0
      common /wav/ ax,ay
      common /correlation/ cor_d
ccccccccccccccccccccccccccccccccccccccccccccccccccccccccc
	auto=0d0
	au_x=0d0
	au_y=0d0
	im=(0d0,1d0)
      cor_d=0d0
c------correlation function-------------------------------
        do j=1,nj
                y=ymin+(j-1)*dy
        do i=1,ni
                x=xmin+(i-1)*dx
      auto=auto+conjg(psi(i,j))*psi0(i,j)*dx*dy
	au_x=au_x+conjg(psi(i,j))*exp(-ax*x**2+im*px0*(x-qx0))*dx*dy
      au_y=au_y+conjg(psi(i,j))*exp(-ay*y**2+im*py0*(x-qy0))*dx*dy
      cor_d=cor_d+abs(psi0(i,j))**2*abs(psi(i,j))**2*dx*dy
      enddo
	enddo
c       yy=yy+abs(psi(i,j))**2*dx*dy*y**2
c	ex=ex+abs(psi(i,j))**2*dx*dy*(x**2*y-y**3/3d0)

	end subroutine