C$Procedure      Q2M ( Quaternion to matrix )

      SUBROUTINE Q2M ( Q, R )

C$ Abstract
C
C     Find the rotation matrix corresponding to a specified unit
C     quaternion.
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      Q ( 0 : 3 )
      DOUBLE PRECISION      R ( 3,  3 )

C$ Brief_I/O
C
C     Variable  I/O  Description
C     --------  ---  --------------------------------------------------
C     Q          I   A unit quaternion.
C     R          O   A rotation matrix corresponding to Q.
C
C$ Detailed_Input
C
C     Q              is a unit-length SPICE-style quaternion. Q has the
C                    property that
C
C                       || Q ||  =  1
C
C                    See the discussion of quaternion styles in
C                    Particulars below.
C
C$ Detailed_Output
C
C     R              is a 3 by 3 rotation matrix representing the same
C                    rotation as does Q. See the discussion titled
C                    "Associating SPICE Quaternions with Rotation
C                    Matrices" in Particulars below.
C
C$ Parameters
C
C     None.
C
C$ Exceptions
C
C     Error free.
C
C     1) If Q is not a unit quaternion, the output matrix M is
C        the rotation matrix that is the result of converting
C        normalized Q to a rotation matrix.
C
C     2) If Q is the zero quaternion, the output matrix M is
C        the identity matrix.
C
C$ Files
C
C     None.
C
C$ Particulars
C
C     If a 4-dimensional vector Q satisfies the equality
C
C        || Q ||   =  1
C
C     or equivalently
C
C            2          2          2          2
C        Q(0)   +   Q(1)   +   Q(2)   +   Q(3)   =  1,
C
C     then we can always find a unit vector A and a scalar r such that
C
C        Q = ( cos(r/2), sin(r/2)A(1), sin(r/2)A(2), sin(r/2)A(3) ).
C
C     We can interpret A and r as the axis and rotation angle of a
C     rotation in 3-space.  If we restrict r to the range [0, pi],
C     then r and A are uniquely determined, except if r = pi.  In this
C     special case, A and -A are both valid rotation axes.
C
C     Every rotation is represented by a unique orthogonal matrix; this
C     routine returns that unique rotation matrix corresponding to Q.
C
C     The SPICELIB routine M2Q is a 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     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 quaternion
C
C               Q = ( sqrt(2)/2, 0, 0, -sqrt(2)/2 )
C
C            to a rotation matrix, we can use the code fragment
C
C               Q(0) =  DSQRT(2)/2.D0
C               Q(1) =  0.D0
C               Q(2) =  0.D0
C               Q(3) = -DSQRT(2)/2.D0
C
C               CALL Q2M ( Q, R )
C
C            The matrix R will be set equal to
C
C               +-              -+
C               |  0     1    0  |
C               |                |
C               | -1     0    0  |.
C               |                |
C               |  0     0    1  |
C               +-              -+
C
C            Why?  Well, Q represents a rotation by some angle r about
C            some axis vector A, where r and A satisfy
C
C               Q =
C
C               ( cos(r/2), sin(r/2)A(1), sin(r/2)A(2), sin(r/2)A(3) ).
C
C            In this example,
C
C               Q = ( sqrt(2)/2, 0, 0, -sqrt(2)/2 ),
C
C            so
C
C               cos(r/2) = sqrt(2)/2.
C
C            Assuming that r is in the interval [0, pi], we must have
C
C               r = pi/2,
C
C            so
C
C               sin(r/2) = sqrt(2)/2.
C
C            Since the second through fourth components of Q represent
C
C               sin(r/2) * A,
C
C            it follows that
C
C               A = ( 0, 0, -1 ).
C
C            So Q represents a transformation that rotates vectors by
C            pi/2 about the negative z-axis.  This is equivalent to a
C            coordinate system rotation of pi/2 about the positive
C            z-axis; and we recognize R as the matrix
C
C               [ pi/2 ] .
C                       3
C
C
C     2)  Finding a set of Euler angles that represent a rotation
C         specified by a quaternion:
C
C            Suppose our rotation R is represented by the quaternion
C            Q.  To find angles TAU, ALPHA, DELTA such that
C
C
C               R  =  [ TAU ]  [ pi/2 - DELTA ]  [ ALPHA ] ,
C                            3                 2          3
C
C            we can use the code fragment
C
C
C               CALL Q2M    ( Q, R )
C
C               CALL M2EUL  ( R,   3,      2,       3,
C              .                   TAU,    DELTA,   ALPHA  )
C
C               DELTA = HALFPI() - DELTA
C
C$ Restrictions
C
C     None.
C
C$ Literature_References
C
C     [1]    NAIF document 179.0, "Rotations and their Habits", by
C            W. L. Taber.
C
C$ Author_and_Institution
C
C     N.J. Bachman   (JPL)
C     W.L. Taber     (JPL)
C     F.S. Turner    (JPL)
C
C$ Version
C
C-    SPICELIB Version 1.1.2, 26-FEB-2008 (NJB)
C
C        Updated header; added information about SPICE
C        quaternion conventions.
C
C-    SPICELIB Version 1.1.1, 13-JUN-2002 (FST)
C
C        Updated the Exceptions section to clarify exceptions that
C        are the result of changes made in the previous version of
C        the routine.
C
C-    SPICELIB Version 1.1.0, 04-MAR-1999 (WLT)
C
C        Added code to handle the case in which the input quaternion
C        is not of length 1.
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     quaternion to matrix
C
C-&

C
C     Local variables
C
      DOUBLE PRECISION      Q01
      DOUBLE PRECISION      Q02
      DOUBLE PRECISION      Q03
      DOUBLE PRECISION      Q12
      DOUBLE PRECISION      Q13
      DOUBLE PRECISION      Q23
      DOUBLE PRECISION      Q1S
      DOUBLE PRECISION      Q2S
      DOUBLE PRECISION      Q3S

      DOUBLE PRECISION      L2
      DOUBLE PRECISION      SHARPN

C
C     If a matrix R represents a rotation of r radians about the unit
C     vector n, we know that R can be represented as
C
C                                           2
C        I  +  sin(r) N  +  [ 1 - cos(r) ] N ,
C
C     where N is the matrix that satisfies
C
C        Nv = n x v
C
C     for all vectors v, namely
C
C             +-                -+
C             |  0    -n     n   |
C             |         3     2  |
C             |                  |
C        N =  |  n     0    -n   |.
C             |   3           1  |
C             |                  |
C             | -n     n     0   |
C             |   2     1        |
C             +-                -+
C
C
C      Define S as
C
C         sin(r/2) N,
C
C      and let our input quaternion Q be
C
C         ( q ,  q ,  q ,  q ).
C            0    1    2    3
C
C      Using the facts that
C
C                             2
C         1 - cos(r)  =  2 sin (r/2)
C
C      and
C
C         sin(r)      =  2 cos(r/2) sin(r/2),
C
C
C      we can express R as
C
C                                      2
C         I  +  2 cos(r/2) S    +   2 S,
C
C      or
C                                2
C         I  +  2 q  S    +   2 S.
C                  0
C
C      Since S is just
C
C         +-                -+
C         |  0    -q     q   |
C         |         3     2  |
C         |                  |
C         |  q     0    -q   |,
C         |   3           1  |
C         |                  |
C         | -q     q     0   |
C         |   2     1        |
C         +-                -+
C
C      our expression for R comes out to
C
C         +-                                                         -+
C         |          2   2                                            |
C         | 1 - 2 ( q + q  )    2( q q  -  q q )     2 ( q q  + q q ) |
C         |          2   3          1 2     0 3           1 3    0 2  |
C         |                                                           |
C         |                              2   2                        |
C         | 2( q q  +  q q )    1 - 2 ( q + q  )     2 ( q q  - q q ) |.
C         |     1 2     0 3              1   3            2 3    0 1  |
C         |                                                           |
C         |                                                   2   2   |
C         | 2( q q  -  q q )    2 ( q q  + q q )     1 - 2 ( q + q  ) |
C         |     1 3     0 2          2 3    0 1               1   2   |
C         +-                                                         -+
C
C
C      For efficiency, we avoid duplicating calculations where possible.
C

      Q01  =  Q(0) * Q(1)
      Q02  =  Q(0) * Q(2)
      Q03  =  Q(0) * Q(3)

      Q12  =  Q(1) * Q(2)
      Q13  =  Q(1) * Q(3)

      Q23  =  Q(2) * Q(3)

      Q1S  =  Q(1) * Q(1)
      Q2S  =  Q(2) * Q(2)
      Q3S  =  Q(3) * Q(3)

C
C     We sharpen the computation by effectively converting Q to
C     a unit quaternion if it isn't one already.
C
      L2   =  Q(0) * Q(0) + Q1S + Q2S + Q3S

      IF ( L2 .NE. 1.0D0 .AND. L2 .NE. 0.0D0 ) THEN

         SHARPN = 1.0D0 / L2

         Q01    = Q01 * SHARPN
         Q02    = Q02 * SHARPN
         Q03    = Q03 * SHARPN

         Q12    = Q12 * SHARPN
         Q13    = Q13 * SHARPN

         Q23    = Q23 * SHARPN

         Q1S    = Q1S * SHARPN
         Q2S    = Q2S * SHARPN
         Q3S    = Q3S * SHARPN

      END IF

      R(1,1) =  1.D0  -  2.D0 * ( Q2S + Q3S )
      R(2,1) =           2.D0 * ( Q12 + Q03 )
      R(3,1) =           2.D0 * ( Q13 - Q02 )

      R(1,2) =           2.D0 * ( Q12 - Q03 )
      R(2,2) =  1.D0  -  2.D0 * ( Q1S + Q3S )
      R(3,2) =           2.D0 * ( Q23 + Q01 )

      R(1,3) =           2.D0 * ( Q13 + Q02 )
      R(2,3) =           2.D0 * ( Q23 - Q01 )
      R(3,3) =  1.D0  -  2.D0 * ( Q1S + Q2S )

      RETURN
      END


module logspace

  use types, only: dp

  implicit none

contains

  impure elemental function log1p(x) result (y)
    real(dp), intent(in) :: x
    real(dp) :: y
    real(dp), volatile :: u, v

    u = 1.0d0 + x
    if (u == 0) then
       y = log(u)
    else
       v = u - 1.0d0
       y = log(u) - (v - x) / u    ! cancels errors with IEEE arithmetic
    end if
  end function log1p

  function logadd(x, y) result (sum)
    use ieee_constants, only: isneginf

    real(dp), intent(in) :: x, y
    real(dp) :: sum

    if (isneginf(x)) then
       sum = y
    else if (isneginf(y)) then
       sum = x
    else
       sum = max(x, y) + log1p(exp(-abs(x - y)))
    end if
  end function logadd

end module logspace


program balistica

    implicit none
    double precision, dimension (0:500) :: x, y, vy, tempo, v
    double precision :: g, ang, dt, vx
    integer :: c

    x(0) = 0
    y(0) = 0
    v(0) = 1
    g = 9.8
    dt = 0.05
    ang = 0.7854

    do c = 1,500
	tempo(c) = dt*c
	vx = v(0)*cos(ang)
	vy(0) = v(0)*sin(ang)
	vy(c) = vy(c-1) - g*dt
	y(c) = vy(c)*dt + y(c-1)
	x(c) = vx*dt + x(c-1)
	v(c) = sqrt((vx**2)+(vy(c)**2))
	write(*,*) x(c), y(c)
    enddo

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

	do c = 1,500
	    write(1,*) x(c), y(c)
	enddo

    close(1)

end program


C$Procedure  DCBRT ( Double precision cube root )

      DOUBLE PRECISION FUNCTION  DCBRT ( X )

C$ Abstract
C
C      Return the cube root of a double precision number.
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     None.
C
C$ Keywords
C
C      MATH,  ROOT
C
C$ Declarations

      DOUBLE PRECISION   X

C$ Brief_I/O
C
C      VARIABLE  I/O  DESCRIPTION
C      --------  ---  --------------------------------------------------
C       X         I     Number whose cube root is desired.
C
C$ Detailed_Input
C
C      X   may be any double precision value.
C
C$ Detailed_Output
C
C      DCBRT   is the cube root of the input value.
C
C$ Parameters
C
C     None.
C
C$ Particulars
C
C      DCBRT calculates the cube root of the input value by using the
C      exponentiation operator to raise the input value to the 1/3
C      power.  This operation, however, is performed on the absolute
C      value of the input variable, and then the sign of the input
C      is transferred to the output value.
C
C      All values of the input variable X should be acceptible to the
C      DCBRT.
C
C$ Examples
C
C      The following table gives sample values of the variable X and
C      DCBRT(X)
C
C       X                        DCBRT(X)
C      --------------------------------------------------------------
C       0.0D0                    0.0D0
C       8.0D0                    2.0D0
C      -1.0D3                   -1.0D1
C
C$ Restrictions
C
C      None
C
C$ Exceptions
C
C      Error free.
C
C$ Files
C
C      None
C
C$ Author_and_Institution
C
C      W.M. Owen       (JPL)
C
C$ Literature_References
C
C      None
C
C$ Version
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, 31-JAN-1990 (WMO)
C
C-&

C$ Index_Entries
C
C     d.p. cube root
C
C-&

      DOUBLE PRECISION THIRD
      PARAMETER (THIRD = 1.D0/3.D0)
C
      DCBRT = DSIGN (DABS(X)**THIRD, X)
C
      RETURN
      END


!##############################################################################
! MODULE globals
!
! This code is published under the GNU General Public License v3
!                         (https://www.gnu.org/licenses/gpl-3.0.en.html)
!
! Authors: Hans Fehr and Fabian Kindermann
!          contact@ce-fortran.com
!
! #VC# VERSION: 1.0  (23 January 2018)
!
!##############################################################################
module globals

    implicit none
    real*8, parameter  :: mu_w = 1d0
    real*8, parameter  :: sig_w = 0.0d0
    integer, parameter :: n_w = 5

    real*8, parameter  :: R = 1d0
    real*8, parameter  :: beta = 1d0
    real*8, parameter  :: gamma = 0.5d0
    real*8, parameter  :: egam = 1d0-1d0/gamma

    real*8 :: w(n_w), weight_w(n_w)
    real*8 :: a(3,n_w), c(3,n_w)
    real*8 :: wag(3,n_w), inc(3,n_w), sav(3,n_w)

contains

    ! utility function of the household
    function utility(x)

        implicit none
        real*8, intent(in) :: x(:)
        real*8 :: utility
        integer :: iw

        ! savings
        a(1, :) = 0d0
        a(2, :) = x(1)
        a(3, :) = x(2:1+n_w)

        ! consumption (ensure consumption > 0)
        c(1,:) = mu_w - a(2,1)
        c(2,:) = R*a(2,1) + w(:) - a(3,:)
        c(3,:) = R*a(3,:)
        c = max(c, 1d-10)

        ! expected utility of periods 2 and 3
        utility = 0d0
        do iw = 1, n_w
            utility = utility+weight_w(iw)*(c(2,iw)**egam + beta*c(3,iw)**egam)
        enddo
        utility = -(c(1,1)**egam + beta*utility)/egam

    end function

    ! calculates the expected value of x
    function E(x)

        implicit none
        real*8, intent(in) :: x(:)
        real*8 :: E
        integer :: iw

        ! calculate expected value
        E = 0d0
        do iw = 1, n_w
            E = e + x(iw)*weight_w(iw)
        enddo

    end function

    ! calculates the standard deviation of x
    function Std(x)

        implicit none
        real*8, intent(in) :: x(:)
        real*8 :: Std
        real*8 :: E
        integer :: iw

        ! calculate expected value
        E = 0d0
        do iw = 1, n_w
            E = E + x(iw)*weight_w(iw)
        enddo

        ! calculate standard deviation
        Std = 0d0
        do iw = 1, n_w
            Std = Std + x(iw)**2*weight_w(iw)
        enddo
        Std = sqrt(max(Std-E**2, 0d0))

    end function

end module


program main
  use AbstractOperators
  !use SimpleVectors
  use DVectors
  use ExampleOperators

  use KrylovSolver
  implicit none
  ! problem vars
  type(Laplacian) :: laplacian_1d
  type(DiagonalMatrix) :: inverse_diagonal_laplacian
  type(dvector) :: sol
  type(dvector) :: rhs
  integer :: n
  integer :: max_iterations=400
  double precision :: tolerance=1.0d-5,h,sign

  ! local vars
  integer :: ierr,i,m,j
  type(dvector) :: res
  n=100
  h=1.0d0/(n-1)

  !! init matrix
  call laplacian_1d%init(n)

  !! allocate solution and rhs and set the latter
  call sol%init(n)
  call rhs%init(n)
  call res%init(n)

  do i=1,n
     call rhs%set(i,0.d0)
  end do
  call rhs%set(1,1.0d0)
  call rhs%set(n,1.0d0)

  do i=1,n
     call sol%set(i,0.d0)
  end do

  !! init diagonal and set values with inverse of
  !! diagonal of the Laplacian
  call inverse_diagonal_laplacian%init(n)
  inverse_diagonal_laplacian%coefficients=1.0d0/(2.0d0)

  write(*,*) norm_dvector(rhs)
  call pcg(laplacian_1d,rhs,sol,ierr,&
       tolerance=1.0d-3,&
       max_iterations=400)!,&
  !preconditi1.0d0r=inverse_diagonal_laplacian)

  !! check results

  call laplacian_1d%apply(sol,res,ierr)
  call axpby_dvector(-1.0d0,rhs,1.0d0,res)
  write(*,*) 'Res=',norm_dvector(res)

end program main


  ! Index of velocity nodes
  i1 = i - sgnvz
  j1 = j - sgnvx

  ! Get local times of surrounding points
  tv = tt(i-sgntz,j)
  te = tt(i,j-sgntx)
  tev = tt(i-sgntz,j-sgntx)

  ! 1D operators (refracted times)
  vref = min( slow(i1,max(j-1,1)), slow(i1,min(j,nx-1)) )
  t1d1 = tv + dz * vref                   ! First dimension (Z axis)
  vref = min( slow(max(i-1,1),j1), slow(min(i,nz-1),j1) )
  t1d2 = te + dx * vref                   ! Second dimension (X axis)

  ! 2D operators
  t2d = Big
  t1 = Big
  t2 = Big
  t3 = Big
  vref = slow(i1,j1)

  ! Choose plane wave or spherical
  ! Test for plane wave
  if ( .not. perturbation .or. ( abs(i-zsi) .gt. epsin .or. abs(j-xsi) .gt. epsin ) ) then

    ! 4 points operator if possible, otherwise do three points
    if ( ( tv .le. te + dx*vref ) .and. ( te .le. tv + dz*vref ) &
      .and. ( te - tev .ge. 0.d0 ) .and. ( tv - tev .ge. 0.d0 ) ) then
      ta = tev + te - tv
      tb = tev - te + tv
      t1 = ( ( tb * dz2i + ta * dx2i ) + sqrt( 4.d0 * vref*vref * ( dz2i + dx2i ) &
           - dz2i * dx2i * ( ta - tb ) * ( ta - tb ) ) ) / ( dz2i + dx2i )

    ! Two 3 points operators
    else if ( ( te - tev ) .le. dz*dz * vref / sqrt( dx*dx + dz*dz ) &
      .and. ( te - tev ) .gt. 0.d0 ) then
      t2 = te + dx * sqrt( vref*vref - ( ( te - tev ) / dz )**2.d0 )

    else if ( ( tv - tev ) .le. dx*dx * vref / sqrt( dx*dx + dz*dz ) &
      .and. ( tv - tev ) .gt. 0.d0 ) then
      t3 = tv + dz * sqrt( vref*vref - ( ( tv - tev ) / dx )**2.d0 )
    end if

  ! Test for spherical
  else
    ! Do spherical operator if conditions ok
    if ( ( tv .lt. te + dx*vref ) .and. ( te .lt. tv + dz*vref ) &
      .and. ( te - tev .ge. 0.d0 ) .and. ( tv - tev .ge. 0.d0 ) ) then
      t0c = t_anad(tzc, txc, i, j, dz, dx, zsa, xsa, vzero)
      tauv = tv - t_ana(i-sgntz, j, dz, dx, zsa, xsa, vzero)
      taue = te - t_ana(i, j-sgntx, dz, dx, zsa, xsa, vzero)
      tauev = tev - t_ana(i-sgntz, j-sgntx, dz, dx, zsa, xsa, vzero)
      ta = tauev + taue - tauv
      tb = tauev - taue + tauv
      apoly = dz2i + dx2i
      bpoly = 4.d0 * ( dfloat(sgntx) * txc * dxi + dfloat(sgntz) * tzc * dzi ) &
              - 2.d0 * ( ta * dx2i + tb * dz2i )
      cpoly = ( ta*ta * dx2i ) + ( tb*tb * dz2i ) &
              - 4.d0 * ( dfloat(sgntx) * txc * dxi * ta + dfloat(sgntz) * tzc * dzi * tb ) &
              + 4.d0 * ( vzero*vzero - vref*vref )
      dpoly = bpoly*bpoly - 4.d0 * apoly * cpoly
      if ( dpoly .ge. 0.d0 ) t1 = 0.5d0 * ( sqrt(dpoly) - bpoly ) / apoly + t0c
      if ( t1-tv .lt. 0.d0 .or. t1-te .lt. 0.d0 ) t1 = Big
    end if

  end if

  t2d = min(t1, t2, t3)

  ! Select minimum time
  time_sol = [ tt(i,j), t1d1, t1d2, t2d ]
  imin = minloc(time_sol, dim = 1)
  tt(i,j) = time_sol(imin)

  ! Compute gradient according to minimum time direction
  if ( present(ttgrad) ) then
    select case(imin)
    case(2)
      ttz(i,j) = sgntz * (tt(i,j)-tv) / dz
      ttx(i,j) = 0.d0
    case(3)
      ttz(i,j) = 0.d0
      ttx(i,j) = sgntx * (tt(i,j)-te) / dx
    case(4)
      ttz(i,j) = sgntz * (tt(i,j)-tv) / dz
      ttx(i,j) = sgntx * (tt(i,j)-te) / dx
    end select
  end if


c
c
c
c
      double precision function fbody(x,y)

      implicit double precision (a-h,o-z)
c     common /userdt/ cfl,gamma,gamma1,xprob,yprob,alpha,Re,iprob,
c    .                ismp,gradThreshold
c
c  negative inside the body (exterior to the domain), positive otherwise.
c

c  wedge geomtry - flat, then 30 degree ramp
      pi = 3.14159265358979d0

c     xwall = 0.506756756756750d0
      xwall = 1.d0/6.d0
      ywall = (3.d0-xwall)*tan(30.d0*pi/180.d0)

      if (x .le. xwall) then
         if (y .gt.0.d0) then
             fbody = 1.d0
         else
             fbody = -1.d0
         endif
      else
        slope = (ywall - 0.d0)/(3.00d0-xwall)
        ybndry = (x-xwall)*slope
        if (y .gt. ybndry) then
           fbody = 1.d0
        else
           fbody = -1.d0
        endif
      endif

      return
      end
c


      SUBROUTINE LAGINT (X,Y,n,xint,yout)
C
C     This subroutine performs lagrangian interpolation
C     within a set of (X,Y) pairs to give the y
C     value corresponding to xint. This program uses a
C     window of 4 data points to perform the interpolation.
C     if the window size needs to be changed, this can be
C     done by changing the indices in the do loops for
C     variables m and j.
C
C     PARAMETERS ARE :
C     X     - array of values of the independent variable
C     Y     - array of function values corresponding to x
C     n     - number of points
C     xint  - the x-value for which estimate of y is desired
C     yout  - the y value returned to caller

      implicit none
      REAL*8 X(n),Y(n),xint,yout,term
      INTEGER i,j,k,m,n

      yout = 0.d0
      do  i = 1,n-1
        if ( xint .ge. X(i) .and. xint .lt. X(i+1) ) k = i
      enddo

      if ( k .lt. 2 ) k = 2
      if ( k .gt. n-2 ) k = n-2

      do m = k-1,k+2
        term = y(m)
        do  j = k-1,k+2
          if ( m .ne. j ) then
            term = term * (xint - X(j))/(X(m) - X(j))
          end if
        enddo
        yout = yout + term
      enddo

      return
      end


program main
  use nrtype
  use distributions
  use metropolis_hastings_mod
  implicit none

  !open(1,file = 'sine.dat')
  !open(2,file = 'sine_cos.dat')
  !open(3,file = 'exp_decay_cos.dat')
  open(4,file = 'bivar_gauss.dat')
  !call circular_met_hast(dist = radial_sine_dist, radial = 2._dp,&
  !                       angular = [0._dp,tau], auto_corr = 5,&
  !                       nbr_samples = int(1e6), file_unit = 1)

 !call circular_met_hast(dist = radial_sine_cos_dist, radial = 2._dp,&
 !                       angular = [0._dp,tau], auto_corr = 5,&
 !                       nbr_samples = int(1e6), file_unit = 2)

 !call circular_met_hast(dist = radial_exp_decay_cos, radial = 2._dp,&
 !                      angular = [0._dp,tau], auto_corr = 5,&
 !                      nbr_samples = int(1e6), file_unit = 3)

 call n_dim_met_hast(dist = bivariate_gaussian, n_dim = 2, &
                     aux = [-.5_dp,-5._dp,7._dp,6._dp,0.5_dp], mutagen = [-2._dp,2._dp], &
                     auto_corr = 5, nbr_samples = int(1e6), file_unit = 4)

 ! Equivalence.
 ! x = x(1)
 ! y = x(2)
 ! rho = aux(1)
 ! mu_x = aux(2)
 ! mu_y = aux(3)
 ! sig_x = aux(4)
 ! sig_y = aux(5)

end program main


      SUBROUTINE SB03OY( DISCR, LTRANS, ISGN, S, LDS, R, LDR, A, LDA,
     $                   SCALE, INFO )
C
C     SLICOT RELEASE 5.7.
C
C     Copyright (c) 2002-2020 NICONET e.V.
C
C     PURPOSE
C
C     To solve for the Cholesky factor  U  of  X,
C
C        op(U)'*op(U) = X,
C
C     where  U  is a two-by-two upper triangular matrix, either the
C     continuous-time two-by-two Lyapunov equation
C                                         2
C         op(S)'*X + X*op(S) = -ISGN*scale *op(R)'*op(R),
C
C     when DISCR = .FALSE., or the discrete-time two-by-two Lyapunov
C     equation
C                                         2
C         op(S)'*X*op(S) - X = -ISGN*scale *op(R)'*op(R),
C
C     when DISCR = .TRUE., where op(K) = K or K' (i.e., the transpose of
C     the matrix K),  S  is a two-by-two matrix with complex conjugate
C     eigenvalues,  R  is a two-by-two upper triangular matrix,
C     ISGN = -1 or 1,  and  scale  is an output scale factor, set less
C     than or equal to 1 to avoid overflow in  X.  The routine also
C     computes two matrices, B and A, so that
C                                   2
C        B*U = U*S  and  A*U = scale *R,  if  LTRANS = .FALSE.,  or
C                                   2
C        U*B = S*U  and  U*A = scale *R,  if  LTRANS = .TRUE.,
C     which are used by the general Lyapunov solver.
C     In the continuous-time case  ISGN*S  must be stable, so that its
C     eigenvalues must have strictly negative real parts.
C     In the discrete-time case  S  must be convergent if ISGN = 1, that
C     is, its eigenvalues must have moduli less than unity, or  S  must
C     be completely divergent if ISGN = -1, that is, its eigenvalues
C     must have moduli greater than unity.
C
C     ARGUMENTS
C
C     Mode Parameters
C
C     DISCR   LOGICAL
C             Specifies the equation to be solved:       2
C             = .FALSE.: op(S)'*X + X*op(S) = -ISGN*scale *op(R)'*op(R);
C                                                        2
C             = .TRUE. : op(S)'*X*op(S) - X = -ISGN*scale *op(R)'*op(R).
C
C     LTRANS  LOGICAL
C             Specifies the form of op(K) to be used, as follows:
C             = .FALSE.:  op(K) = K    (No transpose);
C             = .TRUE. :  op(K) = K**T (Transpose).
C
C     ISGN    INTEGER
C             Specifies the sign of the equation as described before.
C             ISGN may only be 1 or -1.
C
C     Input/Output Parameters
C
C     S       (input/output) DOUBLE PRECISION array, dimension (LDS,2)
C             On entry, S must contain a 2-by-2 matrix.
C             On exit, S contains a 2-by-2 matrix B such that B*U = U*S,
C             if LTRANS = .FALSE., or U*B = S*U, if LTRANS = .TRUE..
C             Notice that if U is nonsingular then
C               B = U*S*inv( U ),  if LTRANS = .FALSE.
C               B = inv( U )*S*U,  if LTRANS = .TRUE..
C
C     LDS     INTEGER
C             The leading dimension of array S.  LDS >= 2.
C
C     R       (input/output) DOUBLE PRECISION array, dimension (LDR,2)
C             On entry, R must contain a 2-by-2 upper triangular matrix.
C             The element R( 2, 1 ) is not referenced.
C             On exit, R contains U, the 2-by-2 upper triangular
C             Cholesky factor of the solution X, X = op(U)'*op(U).
C
C     LDR     INTEGER
C             The leading dimension of array R.  LDR >= 2.
C
C     A       (output) DOUBLE PRECISION array, dimension (LDA,2)
C             A contains a 2-by-2 upper triangular matrix A satisfying
C             A*U/scale = scale*R, if LTRANS = .FALSE., or
C             U*A/scale = scale*R, if LTRANS = .TRUE..
C             Notice that if U is nonsingular then
C               A = scale*scale*R*inv( U ),  if LTRANS = .FALSE.
C               A = scale*scale*inv( U )*R,  if LTRANS = .TRUE..
C
C     LDA     INTEGER
C             The leading dimension of array A.  LDA >= 2.
C
C     SCALE   (output) DOUBLE PRECISION
C             The scale factor, scale, set less than or equal to 1 to
C             prevent the solution overflowing.
C
C     Error Indicator
C
C     INFO    INTEGER
C             = 0:  successful exit;
C             = 1:  if the Lyapunov equation is (nearly) singular
C                   (warning indicator);
C                   if DISCR = .FALSE., this means that while the
C                   matrix S has computed eigenvalues with negative real
C                   parts, it is only just stable in the sense that
C                   small perturbations in S can make one or more of the
C                   eigenvalues have a non-negative real part;
C                   if DISCR = .TRUE., this means that while the
C                   matrix S has computed eigenvalues inside the unit
C                   circle, it is nevertheless only just convergent, in
C                   the sense that small perturbations in S can make one
C                   or more of the eigenvalues lie outside the unit
C                   circle;
C                   perturbed values were used to solve the equation
C                   (but the matrix S is unchanged);
C             = 2:  if DISCR = .FALSE., and ISGN*S is not stable or
C                   if DISCR = .TRUE., ISGN = 1 and S is not convergent
C                   or if DISCR = .TRUE., ISGN = -1 and S is not
C                   completely divergent;
C             = 4:  if S has real eigenvalues.
C
C     NOTE: In the interests of speed, this routine does not check all
C           inputs for errors.
C
C     METHOD
C
C     The LAPACK scheme for solving 2-by-2 Sylvester equations is
C     adapted for 2-by-2 Lyapunov equations, but directly computing the
C     Cholesky factor of the solution.
C
C     REFERENCES
C
C     [1] Hammarling S. J.
C         Numerical solution of the stable, non-negative definite
C         Lyapunov equation.
C         IMA J. Num. Anal., 2, pp. 303-325, 1982.
C
C     NUMERICAL ASPECTS
C
C     The algorithm is backward stable.
C
C     CONTRIBUTOR
C
C     Release 3.0: V. Sima, Katholieke Univ. Leuven, Belgium, Aug. 1997.
C     Supersedes Release 2.0 routine SB03CY by Sven Hammarling,
C     NAG Ltd., United Kingdom, November 1986.
C     Partly based on SB03CY and PLYAP2 by A. Varga, University of
C     Bochum, May 1992.
C
C     REVISIONS
C
C     V. Sima, Dec. 1997, April 1998, Aug. 2012.
C     D. Kressner, ETH Zurich, May 2011.
C
C     KEYWORDS
C
C     Lyapunov equation, orthogonal transformation, real Schur form.
C
C     *****************************************************************
C
C     .. Parameters ..
      DOUBLE PRECISION  ZERO, ONE, TWO, FOUR
      PARAMETER         ( ZERO = 0.0D0, ONE = 1.0D0, TWO = 2.0D0,
     $                    FOUR = 4.0D0 )
C     .. Scalar Arguments ..
      LOGICAL           DISCR, LTRANS
      INTEGER           INFO, ISGN, LDA, LDR, LDS
      DOUBLE PRECISION  SCALE
C     .. Array Arguments ..
      DOUBLE PRECISION  A(LDA,*), R(LDR,*), S(LDS,*)
C     .. Local Scalars ..
      DOUBLE PRECISION  ABSB, ABSG, ABST, ALPHA, BIGNUM, E1, E2, EPS,
     $                  ETA, P1, P3, P3I, P3R, S11, S12, S21, S22,
     $                  SCALOC, SGN, SMIN, SMLNUM, SNP, SNQ, SNT, TEMPI,
     $                  TEMPR, V1, V3
C     .. Local Arrays ..
      DOUBLE PRECISION  CSP(2), CSQ(2), CST(2), DELTA(2), DP(2), DT(2),
     $                  G(2), GAMMA(2), P2(2), T(2), TEMP(2), V2(2),
     $                  X11(2), X12(2), X21(2), X22(2), Y(2)
C     .. External Functions ..
      DOUBLE PRECISION  DLAMCH, DLAPY2, DLAPY3
      EXTERNAL          DLAMCH, DLAPY2, DLAPY3
C     .. External Subroutines ..
      EXTERNAL          DLABAD, DLANV2, SB03OV
C     .. Intrinsic Functions ..
      INTRINSIC         ABS, MAX, SIGN, SQRT
C     .. Executable Statements ..
C
C     The comments in this routine refer to notation and equation
C     numbers in sections 6 and 10 of [1].
C
C     Find the eigenvalue  lambda = E1 - i*E2  of s11.
C
      INFO = 0
      SGN = ISGN
      S11 = S(1,1)
      S12 = S(1,2)
      S21 = S(2,1)
      S22 = S(2,2)
C
C     Set constants to control overflow.
C
      EPS = DLAMCH( 'P' )
      SMLNUM = DLAMCH( 'S' )
      BIGNUM = ONE / SMLNUM
      CALL DLABAD( SMLNUM, BIGNUM )
      SMLNUM = SMLNUM*FOUR / EPS
      BIGNUM = ONE / SMLNUM
C
      SMIN = MAX( SMLNUM, EPS*MAX( ABS( S11 ), ABS( S12 ),
     $                             ABS( S21 ), ABS( S22 ) ) )
      SCALE = ONE
C
      CALL DLANV2( S11, S12, S21, S22, TEMPR, TEMPI, E1, E2, CSP, CSQ )
      IF ( TEMPI.EQ.ZERO ) THEN
         INFO = 4
         RETURN
      END IF
      ABSB = DLAPY2( E1, E2 )
      IF ( DISCR ) THEN
         IF ( SGN*( ABSB - ONE ).GE.ZERO ) THEN
            INFO = 2
            RETURN
         END IF
      ELSE
         IF ( SGN*E1.GE.ZERO ) THEN
            INFO = 2
            RETURN
         END IF
      END IF
C
C     Compute the cos and sine that define  Qhat.  The sine is real.
C
      TEMP(1) = S(1,1) - E1
      TEMP(2) = E2
      IF ( LTRANS ) TEMP(2) = -E2
      CALL SB03OV( TEMP, S(2,1), SMLNUM, CSQ, SNQ )
C
C     beta in (6.9) is given by  beta = E1 + i*E2,  compute  t.
C
      TEMP(1) = CSQ(1)*S(1,2) - SNQ*S(1,1)
      TEMP(2) = CSQ(2)*S(1,2)
      TEMPR   = CSQ(1)*S(2,2) - SNQ*S(2,1)
      TEMPI   = CSQ(2)*S(2,2)
      T(1)    = CSQ(1)*TEMP(1) - CSQ(2)*TEMP(2) + SNQ*TEMPR
      T(2)    = CSQ(1)*TEMP(2) + CSQ(2)*TEMP(1) + SNQ*TEMPI
C
      IF ( LTRANS ) THEN
C                                                         (     -- )
C        Case op(M) = M'.  Note that the modified  R  is  ( p3  p2 ).
C                                                         ( 0   p1 )
C
C        Compute the cos and sine that define  Phat.
C
         TEMP(1) =  CSQ(1)*R(2,2) - SNQ*R(1,2)
         TEMP(2) = -CSQ(2)*R(2,2)
         CALL SB03OV( TEMP, -SNQ*R(1,1), SMLNUM, CSP, SNP )
C
C        Compute p1, p2 and p3 of the relation corresponding to (6.11).
C
         P1 = TEMP(1)
         TEMP(1) =  CSQ(1)*R(1,2) + SNQ*R(2,2)
         TEMP(2) = -CSQ(2)*R(1,2)
         TEMPR   =  CSQ(1)*R(1,1)
         TEMPI   = -CSQ(2)*R(1,1)
         P2(1)   =  CSP(1)*TEMP(1) - CSP(2)*TEMP(2) + SNP*TEMPR
         P2(2)   = -CSP(1)*TEMP(2) - CSP(2)*TEMP(1) - SNP*TEMPI
         P3R     =  CSP(1)*TEMPR   + CSP(2)*TEMPI   - SNP*TEMP(1)
         P3I     =  CSP(1)*TEMPI   - CSP(2)*TEMPR   - SNP*TEMP(2)
      ELSE
C
C        Case op(M) = M.
C
C        Compute the cos and sine that define  Phat.
C
         TEMP(1) = CSQ(1)*R(1,1) + SNQ*R(1,2)
         TEMP(2) = CSQ(2)*R(1,1)
         CALL SB03OV( TEMP, SNQ*R(2,2), SMLNUM, CSP, SNP )
C
C        Compute p1, p2 and p3 of (6.11).
C
         P1 = TEMP(1)
         TEMP(1) = CSQ(1)*R(1,2) - SNQ*R(1,1)
         TEMP(2) = CSQ(2)*R(1,2)
         TEMPR   = CSQ(1)*R(2,2)
         TEMPI   = CSQ(2)*R(2,2)
         P2(1)   = CSP(1)*TEMP(1) - CSP(2)*TEMP(2) + SNP*TEMPR
         P2(2)   = CSP(1)*TEMP(2) + CSP(2)*TEMP(1) + SNP*TEMPI
         P3R     = CSP(1)*TEMPR   + CSP(2)*TEMPI   - SNP*TEMP(1)
         P3I     = CSP(2)*TEMPR   - CSP(1)*TEMPI   + SNP*TEMP(2)
      END IF
C
C     Make  p3  real by multiplying by  conjg ( p3 )/abs( p3 )  to give
C
C     p3 := abs( p3 ).
C
      IF ( P3I.EQ.ZERO ) THEN
         P3 = ABS( P3R )
         DP(1) = SIGN( ONE, P3R )
         DP(2) = ZERO
      ELSE
         P3 = DLAPY2( P3R, P3I )
         DP(1) =  P3R/P3
         DP(2) = -P3I/P3
      END IF
C
C     Now compute the quantities v1, v2, v3 and y in (6.13) - (6.15),
C     or (10.23) - (10.25). Care is taken to avoid overflows.
C
      IF ( DISCR ) THEN
         ALPHA = SQRT( ABS( ONE - ABSB )*( ONE + ABSB ) )
      ELSE
         ALPHA = SQRT( ABS( TWO*E1 ) )
      END IF
C
      SCALOC = ONE
      IF( ALPHA.LT.SMIN ) THEN
         ALPHA = SMIN
         INFO = 1
      END IF
      ABST = ABS( P1 )
      IF( ALPHA.LT.ONE .AND. ABST.GT.ONE ) THEN
         IF( ABST.GT.BIGNUM*ALPHA )
     $      SCALOC = ONE / ABST
      END IF
      IF( SCALOC.NE.ONE ) THEN
         P1    = SCALOC*P1
         P2(1) = SCALOC*P2(1)
         P2(2) = SCALOC*P2(2)
         P3    = SCALOC*P3
         SCALE = SCALOC*SCALE
      END IF
      V1 = P1/ALPHA
C
      IF ( DISCR ) THEN
         G(1) = ( ONE - E1 )*( ONE + E1 ) + E2**2
         G(2) = -TWO*E1*E2
         ABSG =  DLAPY2( G(1), G(2) )
         SCALOC = ONE
         IF( ABSG.LT.SMIN ) THEN
            ABSG = SMIN
            INFO = 1
         END IF
         TEMP(1) = SGN*ALPHA*P2(1) + V1*( E1*T(1) - E2*T(2) )
         TEMP(2) = SGN*ALPHA*P2(2) + V1*( E1*T(2) + E2*T(1) )
         ABST    = MAX( ABS( TEMP(1) ), ABS( TEMP(2) ) )
         IF( ABSG.LT.ONE .AND. ABST.GT.ONE ) THEN
            IF( ABST.GT.BIGNUM*ABSG )
     $         SCALOC = ONE / ABST
         END IF
         IF( SCALOC.NE.ONE ) THEN
            V1      = SCALOC*V1
            TEMP(1) = SCALOC*TEMP(1)
            TEMP(2) = SCALOC*TEMP(2)
            P1      = SCALOC*P1
            P2(1)   = SCALOC*P2(1)
            P2(2)   = SCALOC*P2(2)
            P3      = SCALOC*P3
            SCALE   = SCALOC*SCALE
         END IF
         TEMP(1) = TEMP(1)/ABSG
         TEMP(2) = TEMP(2)/ABSG
C
         SCALOC = ONE
         V2(1)  = G(1)*TEMP(1) + G(2)*TEMP(2)
         V2(2)  = G(1)*TEMP(2) - G(2)*TEMP(1)
         ABST   = MAX( ABS( V2(1) ), ABS( V2(2) ) )
         IF( ABSG.LT.ONE .AND. ABST.GT.ONE ) THEN
            IF( ABST.GT.BIGNUM*ABSG )
     $         SCALOC = ONE / ABST
         END IF
         IF( SCALOC.NE.ONE ) THEN
            V1    = SCALOC*V1
            V2(1) = SCALOC*V2(1)
            V2(2) = SCALOC*V2(2)
            P1    = SCALOC*P1
            P2(1) = SCALOC*P2(1)
            P2(2) = SCALOC*P2(2)
            P3    = SCALOC*P3
            SCALE = SCALOC*SCALE
         END IF
         V2(1) = V2(1)/ABSG
         V2(2) = V2(2)/ABSG
C
         SCALOC  = ONE
         TEMP(1) = P1*T(1) - TWO*E2*P2(2)
         TEMP(2) = P1*T(2) + TWO*E2*P2(1)
         ABST    = MAX( ABS( TEMP(1) ), ABS( TEMP(2) ) )
         IF( ABSG.LT.ONE .AND. ABST.GT.ONE ) THEN
            IF( ABST.GT.BIGNUM*ABSG )
     $         SCALOC = ONE / ABST
         END IF
         IF( SCALOC.NE.ONE ) THEN
            TEMP(1) = SCALOC*TEMP(1)
            TEMP(2) = SCALOC*TEMP(2)
            V1      = SCALOC*V1
            V2(1)   = SCALOC*V2(1)
            V2(2)   = SCALOC*V2(2)
            P3      = SCALOC*P3
            SCALE   = SCALOC*SCALE
         END IF
         TEMP(1) = TEMP(1)/ABSG
         TEMP(2) = TEMP(2)/ABSG
C
         SCALOC  = ONE
         Y(1)    = -( G(1)*TEMP(1) + G(2)*TEMP(2) )
         Y(2)    = -( G(1)*TEMP(2) - G(2)*TEMP(1) )
         ABST    = MAX( ABS( Y(1) ), ABS( Y(2) ) )
         IF( ABSG.LT.ONE .AND. ABST.GT.ONE ) THEN
            IF( ABST.GT.BIGNUM*ABSG )
     $         SCALOC = ONE / ABST
         END IF
         IF( SCALOC.NE.ONE ) THEN
            Y(1)  = SCALOC*Y(1)
            Y(2)  = SCALOC*Y(2)
            V1    = SCALOC*V1
            V2(1) = SCALOC*V2(1)
            V2(2) = SCALOC*V2(2)
            P3    = SCALOC*P3
            SCALE = SCALOC*SCALE
         END IF
         Y(1) = Y(1)/ABSG
         Y(2) = Y(2)/ABSG
      ELSE
C
         SCALOC = ONE
         IF( ABSB.LT.SMIN ) THEN
            ABSB = SMIN
            INFO = 1
         END IF
         TEMP(1) = SGN*ALPHA*P2(1) + V1*T(1)
         TEMP(2) = SGN*ALPHA*P2(2) + V1*T(2)
         ABST    = MAX( ABS( TEMP(1) ), ABS( TEMP(2) ) )
         IF( ABSB.LT.ONE .AND. ABST.GT.ONE ) THEN
            IF( ABST.GT.BIGNUM*ABSB )
     $         SCALOC = ONE / ABST
         END IF
         IF( SCALOC.NE.ONE ) THEN
            V1      = SCALOC*V1
            TEMP(1) = SCALOC*TEMP(1)
            TEMP(2) = SCALOC*TEMP(2)
            P2(1)   = SCALOC*P2(1)
            P2(2)   = SCALOC*P2(2)
            P3      = SCALOC*P3
            SCALE   = SCALOC*SCALE
         END IF
         TEMP(1) = TEMP(1)/( TWO*ABSB )
         TEMP(2) = TEMP(2)/( TWO*ABSB )
         SCALOC  = ONE
         V2(1)   = -( E1*TEMP(1) + E2*TEMP(2) )
         V2(2)   = -( E1*TEMP(2) - E2*TEMP(1) )
         ABST = MAX( ABS( V2(1) ), ABS( V2(2) ) )
         IF( ABSB.LT.ONE .AND. ABST.GT.ONE ) THEN
            IF( ABST.GT.BIGNUM*ABSB )
     $         SCALOC = ONE / ABST
         END IF
         IF( SCALOC.NE.ONE ) THEN
            V1    = SCALOC*V1
            V2(1) = SCALOC*V2(1)
            V2(2) = SCALOC*V2(2)
            P2(1) = SCALOC*P2(1)
            P2(2) = SCALOC*P2(2)
            P3    = SCALOC*P3
            SCALE = SCALOC*SCALE
         END IF
         V2(1) = V2(1)/ABSB
         V2(2) = V2(2)/ABSB
         Y(1)  = P2(1) - ALPHA*V2(1)
         Y(2)  = P2(2) - ALPHA*V2(2)
      END IF
C
      SCALOC = ONE
      V3     = DLAPY3( P3, Y(1), Y(2) )
      IF( ALPHA.LT.ONE .AND. V3.GT.ONE ) THEN
         IF( V3.GT.BIGNUM*ALPHA )
     $      SCALOC = ONE / V3
      END IF
      IF( SCALOC.NE.ONE ) THEN
         V1    = SCALOC*V1
         V2(1) = SCALOC*V2(1)
         V2(2) = SCALOC*V2(2)
         V3    = SCALOC*V3
         P3    = SCALOC*P3
         SCALE = SCALOC*SCALE
      END IF
      V3 = V3/ALPHA
C
      IF ( LTRANS ) THEN
C
C        Case op(M) = M'.
C
C        Form  X = conjg( Qhat' )*v11.
C
         X11(1) =  CSQ(1)*V3
         X11(2) =  CSQ(2)*V3
         X21(1) =  SNQ*V3
         X12(1) =  CSQ(1)*V2(1) + CSQ(2)*V2(2) - SNQ*V1
         X12(2) = -CSQ(1)*V2(2) + CSQ(2)*V2(1)
         X22(1) =  CSQ(1)*V1 + SNQ*V2(1)
         X22(2) = -CSQ(2)*V1 - SNQ*V2(2)
C
C        Obtain u11 from the RQ-factorization of X. The conjugate of
C        X22 should be taken.
C
         X22(2) = -X22(2)
         CALL SB03OV( X22, X21(1), SMLNUM, CST, SNT )
         R(2,2) = X22(1)
         R(1,2) = CST(1)*X12(1) - CST(2)*X12(2) + SNT*X11(1)
         TEMPR  = CST(1)*X11(1) + CST(2)*X11(2) - SNT*X12(1)
         TEMPI  = CST(1)*X11(2) - CST(2)*X11(1) - SNT*X12(2)
         IF ( TEMPI.EQ.ZERO ) THEN
            R(1,1) = ABS( TEMPR )
            DT(1)  = SIGN( ONE, TEMPR )
            DT(2)  = ZERO
         ELSE
            R(1,1) =  DLAPY2( TEMPR, TEMPI )
            DT(1)  =  TEMPR/R(1,1)
            DT(2)  = -TEMPI/R(1,1)
         END IF
      ELSE
C
C        Case op(M) = M.
C
C        Now form  X = v11*conjg( Qhat' ).
C
         X11(1) =  CSQ(1)*V1 - SNQ*V2(1)
         X11(2) = -CSQ(2)*V1 + SNQ*V2(2)
         X21(1) = -SNQ*V3
         X12(1) =  CSQ(1)*V2(1) + CSQ(2)*V2(2) + SNQ*V1
         X12(2) = -CSQ(1)*V2(2) + CSQ(2)*V2(1)
         X22(1) =  CSQ(1)*V3
         X22(2) =  CSQ(2)*V3
C
C        Obtain u11 from the QR-factorization of X.
C
         CALL SB03OV( X11, X21(1), SMLNUM, CST, SNT )
         R(1,1) = X11(1)
         R(1,2) = CST(1)*X12(1) + CST(2)*X12(2) + SNT*X22(1)
         TEMPR  = CST(1)*X22(1) - CST(2)*X22(2) - SNT*X12(1)
         TEMPI  = CST(1)*X22(2) + CST(2)*X22(1) - SNT*X12(2)
         IF ( TEMPI.EQ.ZERO ) THEN
            R(2,2) = ABS( TEMPR )
            DT(1)  = SIGN( ONE, TEMPR )
            DT(2)  = ZERO
         ELSE
            R(2,2) =  DLAPY2( TEMPR, TEMPI )
            DT(1)  =  TEMPR/R(2,2)
            DT(2)  = -TEMPI/R(2,2)
         END IF
      END IF
C
C     The computations below are not needed when B and A are not
C     useful. Compute delta, eta and gamma as in (6.21) or (10.26).
C
      IF ( ( ABS( Y(1) ).LT.SMLNUM ).AND.( ABS( Y(2) ).LE.SMLNUM ) )
     $      THEN
         DELTA(1) = ZERO
         DELTA(2) = ZERO
         GAMMA(1) = ZERO
         GAMMA(2) = ZERO
         ETA = ALPHA
      ELSE
         DELTA(1) =  Y(1)/V3
         DELTA(2) =  Y(2)/V3
         GAMMA(1) = -ALPHA*DELTA(1)
         GAMMA(2) = -ALPHA*DELTA(2)
         ETA = P3/V3
         IF ( DISCR ) THEN
            TEMPR    = E1*DELTA(1) - E2*DELTA(2)
            DELTA(2) = E1*DELTA(2) + E2*DELTA(1)
            DELTA(1) = TEMPR
         END IF
      END IF
C
      IF ( LTRANS ) THEN
C
C        Case op(M) = M'.
C
C        Find  X = conjg( That' )*( inv( v11 )*s11hat*v11 ).
C        ( Defer the scaling.)
C
         X11(1) =  CST(1)*E1 + CST(2)*E2
         X11(2) = -CST(1)*E2 + CST(2)*E1
         X21(1) =  SNT*E1
         X21(2) = -SNT*E2
         X12(1) =  SGN*(  CST(1)*GAMMA(1) + CST(2)*GAMMA(2) ) - SNT*E1
         X12(2) =  SGN*( -CST(1)*GAMMA(2) + CST(2)*GAMMA(1) ) - SNT*E2
         X22(1) =  CST(1)*E1 + CST(2)*E2 + SGN*SNT*GAMMA(1)
         X22(2) =  CST(1)*E2 - CST(2)*E1 - SGN*SNT*GAMMA(2)
C
C        Now find  B = X*That. ( Include the scaling here.)
C
         S(1,1) = CST(1)*X11(1) + CST(2)*X11(2) - SNT*X12(1)
         TEMPR  = CST(1)*X21(1) + CST(2)*X21(2) - SNT*X22(1)
         TEMPI  = CST(1)*X21(2) - CST(2)*X21(1) - SNT*X22(2)
         S(2,1) = DT(1)*TEMPR   - DT(2)*TEMPI
         TEMPR  = CST(1)*X12(1) - CST(2)*X12(2) + SNT*X11(1)
         TEMPI  = CST(1)*X12(2) + CST(2)*X12(1) + SNT*X11(2)
         S(1,2) = DT(1)*TEMPR   + DT(2)*TEMPI
         S(2,2) = CST(1)*X22(1) - CST(2)*X22(2) + SNT*X21(1)
C
C        Form  X = ( inv( v11 )*p11 )*conjg( Phat' ).
C
         TEMPR  =  DP(1)*ETA
         TEMPI  = -DP(2)*ETA
         X11(1) =  CSP(1)*TEMPR - CSP(2)*TEMPI + SNP*DELTA(1)
         X11(2) =  CSP(1)*TEMPI + CSP(2)*TEMPR - SNP*DELTA(2)
         X21(1) =  SNP*ALPHA
         X12(1) = -SNP*TEMPR + CSP(1)*DELTA(1) - CSP(2)*DELTA(2)
         X12(2) = -SNP*TEMPI - CSP(1)*DELTA(2) - CSP(2)*DELTA(1)
         X22(1) =  CSP(1)*ALPHA
         X22(2) = -CSP(2)*ALPHA
C
C        Finally form  A = conjg( That' )*X.
C
         TEMPR  = CST(1)*X11(1) - CST(2)*X11(2) - SNT*X21(1)
         TEMPI  = CST(1)*X22(2) + CST(2)*X22(1)
         A(1,1) = DT(1)*TEMPR   + DT(2)*TEMPI
         TEMPR  = CST(1)*X12(1) - CST(2)*X12(2) - SNT*X22(1)
         TEMPI  = CST(1)*X12(2) + CST(2)*X12(1) - SNT*X22(1)
         A(1,2) = DT(1)*TEMPR   + DT(2)*TEMPI
         A(2,1) = ZERO
         A(2,2) = CST(1)*X22(1) + CST(2)*X22(2) + SNT*X12(1)
      ELSE
C
C        Case op(M) = M.
C
C        Find  X = That*( v11*s11hat*inv( v11 ) ). ( Defer the scaling.)
C
         X11(1) =  CST(1)*E1 + CST(2)*E2
         X11(2) =  CST(1)*E2 - CST(2)*E1
         X21(1) = -SNT*E1
         X21(2) = -SNT*E2
         X12(1) =  SGN*(  CST(1)*GAMMA(1) - CST(2)*GAMMA(2) ) + SNT*E1
         X12(2) =  SGN*( -CST(1)*GAMMA(2) - CST(2)*GAMMA(1) ) - SNT*E2
         X22(1) =  CST(1)*E1 + CST(2)*E2 - SGN*SNT*GAMMA(1)
         X22(2) = -CST(1)*E2 + CST(2)*E1 + SGN*SNT*GAMMA(2)
C
C        Now find  B = X*conjg( That' ). ( Include the scaling here.)
C
         S(1,1) = CST(1)*X11(1) - CST(2)*X11(2) + SNT*X12(1)
         TEMPR  = CST(1)*X21(1) - CST(2)*X21(2) + SNT*X22(1)
         TEMPI  = CST(1)*X21(2) + CST(2)*X21(1) + SNT*X22(2)
         S(2,1) = DT(1)*TEMPR   - DT(2)*TEMPI
         TEMPR  = CST(1)*X12(1) + CST(2)*X12(2) - SNT*X11(1)
         TEMPI  = CST(1)*X12(2) - CST(2)*X12(1) - SNT*X11(2)
         S(1,2) = DT(1)*TEMPR   + DT(2)*TEMPI
         S(2,2) = CST(1)*X22(1) + CST(2)*X22(2) - SNT*X21(1)
C
C        Form  X = Phat*( p11*inv( v11 ) ).
C
         TEMPR  =  DP(1)*ETA
         TEMPI  = -DP(2)*ETA
         X11(1) =  CSP(1)*ALPHA
         X11(2) =  CSP(2)*ALPHA
         X21(1) =  SNP*ALPHA
         X12(1) =  CSP(1)*DELTA(1) + CSP(2)*DELTA(2) - SNP*TEMPR
         X12(2) = -CSP(1)*DELTA(2) + CSP(2)*DELTA(1) - SNP*TEMPI
         X22(1) =  CSP(1)*TEMPR + CSP(2)*TEMPI + SNP*DELTA(1)
         X22(2) =  CSP(1)*TEMPI - CSP(2)*TEMPR - SNP*DELTA(2)
C
C        Finally form  A = X*conjg( That' ).
C
         A(1,1) = CST(1)*X11(1) - CST(2)*X11(2) + SNT*X12(1)
         A(2,1) = ZERO
         A(1,2) = CST(1)*X12(1) + CST(2)*X12(2) - SNT*X11(1)
         TEMPR  = CST(1)*X22(1) + CST(2)*X22(2) - SNT*X21(1)
         TEMPI  = CST(1)*X22(2) - CST(2)*X22(1)
         A(2,2) = DT(1)*TEMPR   + DT(2)*TEMPI
      END IF
C
      IF( SCALE.NE.ONE ) THEN
         A(1,1) = SCALE*A(1,1)
         A(1,2) = SCALE*A(1,2)
         A(2,2) = SCALE*A(2,2)
      END IF
C
      RETURN
C *** Last line of SB03OY ***
      END


	SUBROUTINE VC_ETAC  ( p, ps, zs, ptop, nxy, eta, iret )
C************************************************************************
C* VC_ETAC								*
C*									*
C* This subroutine computes eta coordinate values.			*
C*									*
C* VC_ETAC ( P, PS, ZS, PTOP, NXY, ETA,  IRET )				*
C*									*
C* Input parameters:							*
C*	P  (NXY)	REAL		Pressure			*
C*	PS (NXY)	REAL		Surface pressure		*
C*	ZS (NXY)	REAL		Surface height			*
C*	PTOP		REAL		Top pressure			*
C*	NXY		INTEGER		Number of grid points		*
C*									*
C* Output parameters:							*
C*	ETA (NXY)	REAL		Output eta values * 10000	*
C*	IRET		INTEGER		Return code			*
C*					  0 = normal return		*
C*					 +1 = all missing		*
C**									*
C* Log:									*
C* K. Brill/NMC		06/92						*
C* R. Miller/COMET	06/94	Changed 100000 to 10000 for eta and sgma*
C* T. Lee/GSC		12/99	Used TMCK				*
C* J. Wu/GSC            07/00   Moved INCLUDE 'ERMISS.FNC' before the   *
C*                              DATA statement                          *
C************************************************************************
	INCLUDE		'GEMPRM.PRM'
C*
	REAL		p (*), ps (*), zs (*), eta (*)
C*
	INCLUDE		'ERMISS.FNC'
	DATA		po, tlr / 1013.25, .0065 /
	DATA		expo / 5.25612 /
C*
C-----------------------------------------------------------------------
	iret = 0
C
C*	Compute the eta coordinate values.
C
	to = TMCK + 15.
	icnt = 0
	DO i = 1, nxy
	    IF ( ERMISS ( p (i) ) .or. ERMISS ( ps (i) ) .or.
     +		 ERMISS ( zs (i) ) ) THEN
		eta (i) = RMISSD
		icnt = icnt + 1
	    ELSE
		sgma  = ( p (i) - ptop ) / ( ps (i) - ptop )
		aa    = 1. - tlr * zs (i) / to
		prefz = po * aa ** expo
		eee   = ( prefz - ptop ) / ( po - ptop )
		eta (i) = eee * sgma * 10000.
	    END IF
	END DO
C*
	IF ( icnt .eq. nxy ) iret = +1
C*
	RETURN
	END


!===============================================================================
program test_SHTOOLS
!===============================================================================
!  Test the use of SHTOOLS

   use shtools
   use spherical_geometry, only: sphere_sample

   implicit none

   integer :: lmax = 360/5
   real(8),parameter :: pi = atan2(1._8,1._8)*4._8
   real(8) :: dd=5. ! approx distance between knot points
   real(8) :: sigma=0.2  ! Gives half-width of 10 degrees
   real(8),allocatable,dimension(:) :: lon,lat,c
   integer :: n  ! number of points
   real(8) :: val,vlon,vlat
   integer :: i,j
   character(len=20) :: arg

   real(8),allocatable :: cilm(:,:,:)

!  Get lmax from the command line, if supplied
   if (command_argument_count() > 0) then
      call get_command_argument(1,arg)
      read(arg,*) lmax
   endif

!  Create the knot points
   call sphere_sample(dd,lon,lat,n)

!  Make room for the values at each knot and the spherical harmonics
   allocate(c(n))
   allocate(cilm(2,lmax+1,lmax+1))

!  Fill the points with a bulge at the equator, width controlled by sigma
   c = exp((-(lon*pi/180._8)**2 - (lat*pi/180._8)**2)/sigma**2)

!  Alternatively, create a checkerboard
!   s%c = sign(1._8,cos(4._8*s%phi*pi/180._8)*cos(4._8*s%theta*pi/180._8))

!  Make the shape sharp sided
   where (c > 0.5)
      c = 1.
   elsewhere
      c = 0.
   endwhere

!  Fit the spherical harmonics
   write(0,'(a,i0.0,a)') 'test_SHTOOLS: Fitting spherical harmonics with lmax = ',lmax,'...'
   call SHExpandLSQ(cilm,c,lat,lon,n,lmax)
   write(0,'(a)') 'test_SHTOOLS: Finished fitting harmonics'

   write(0,'(a)') 'test_SHTOOLS: Evaluating interpolated values...'
!  Write out points on a regular lon,lat grid
outer: do j=-45,45,1
          do i=-45,45,1
             vlon = real(i)
             vlat = real(j)
             val = MakeGridPoint(cilm,lmax,vlat,vlon)
             write(*,*) i,j,val
          enddo
       enddo outer

end program


!  DLAASRT
!
!  Purpose:
!  ========
!
!  Sort the array D(1:N) in absolute-value descending order and
!  record the permutation in array P(1:N) s.t. new D(j) = old D(P(j))
!
!  Arguments
!  =========
!
!  N        (input) INTEGER
!           The length of the array D.
!
!  D        (input/output) DOUBLE PRECISION array, dimension (N)
!           On entry, the array to be sorted.
!           On exit, D has been sorted into absolute-value descending
!           order.
!
!  P        (output) INTEGER array, dimension (N)
!           P contains the permutation vector.
!
      SUBROUTINE DLAASRT( N, D, P )
!
      INTEGER, INTENT(IN) :: N
      DOUBLE PRECISION, INTENT(INOUT) :: D(N)
      INTEGER, INTENT(OUT) :: P(N)
!
      INTEGER :: I, J, TMP2
      DOUBLE PRECISION :: TMP1
      LOGICAL :: SWAPPED
!
      SWAPPED = .TRUE.
      J = 0
      DO I = 1,N
         P(I) = I
      ENDDO
      DO WHILE (SWAPPED)
        SWAPPED = .FALSE.
        J = J+1
        DO I = 1, N-J
           IF(ABS(D(I))<ABS(D(I+1))) THEN
              TMP1 = D(I)
              TMP2 = P(I)
              D(I) = D(I+1)
              P(I) = P(I+1)
              D(I+1) = TMP1
              P(I+1) = TMP2
              SWAPPED = .TRUE.
           END IF
        END DO
      END DO
      END


!! Savitzky-Golay smoothing routine
subroutine sgolay(xin,xlen,n,order,xout)
  implicit none
  !xin: data in
  !n: left and right length: your filter width is 2n+1
  !order: 0 = smooth the function, 1 = first derivative
  !xout: output
  integer, intent(in) :: n, order,xlen
  double precision :: xin(xlen), xout(xlen), xbig(xlen+2*n+1)
  double precision :: coeffs(-n:n), norm
  integer ::  ii, jj

  call get_sgolay_coeffs(n,order,coeffs,norm)

  ! print*,n, order, coeffs, norm

  xbig(1:n) = xin(1)
  xbig(n+xlen+1:2*n+xlen) = xin(xlen)
  xbig(n+1:n+xlen)= xin(:)

  ! print*,xbig

  xout = 0.
  do ii = 1,xlen
    do jj = -n,n
      xout(ii) = xout(ii)+coeffs(jj)*xbig(ii+n+jj+1)
    end do
  end do
  xout = xout/norm

return
end subroutine sgolay

subroutine get_sgolay_coeffs(n,order,coeffs,norm)
  implicit none
  integer, intent(in) :: n, order
  double precision :: coeffs(2*n+1), norm

  select case (order)
  case (0)
    select case (n)
      case (2)
        coeffs = (/ -3., 12., 17., 12., -3./)
        norm = 35.
      case (3)
        coeffs = (/ -2., 3., 6., 7., 6., 3., -2./)
        norm = 21.
      case (4)
        coeffs = (/-21., 14., 39., 54., 59., 54., 39., 14., -21./)
        norm = 231
      case default
        stop "Problem with sgolay"
    end select
  case (1)
    select case (n)
    case (2)
      coeffs = (/ -2., -1., 0., 1., 2./)
      norm = 10.
    case (3)
      coeffs = (/ -3., -2., -1., 0., 2., 1., 3./)
      norm = 28
    case (4)
      coeffs = (/ -4., -3., -2., -1., 0., 1., 2., 3., 4./)
      norm = 60.
    case default
      stop "Problem with sgolay"
    end select
  end select
return
end subroutine get_sgolay_coeffs


      program test
      implicit none
      ! 定义格点信息
      integer :: nr ! 高斯积分需要用的变量
      integer :: i
      real*8,allocatable,dimension(:) :: rr,rrw ! 高斯积分需要用的变量
      real*8 :: f,f1
      nr=40  !高斯格点数
      allocate(rr(1:nr),rrw(1:nr))
      call gauleg(nr,0.0d0,40.0d0,rr,rrw) ! 初始化高斯积分，输出权重与高斯点

      write(*,*)rrw(2)

      !计算从0到40的x^2的积分
      f=0.0d0
      do i=1,nr
          f=rr(i)**2*rrw(i)+f         !使用输出的格点与权重进行积分
      end do
      f1=40.d0**3/3.d0             !计算理论值
      write(*,*)f,f1
      end program

      SUBROUTINE gauleg(N,x1,x2,X,W)
        IMPLICIT NONE
        INTEGER N
        REAL*8 x1,x2,X(N),W(N)
        REAL*8 z1,z,xm,xl,pp,p3,p2,p1,pi,tol
        INTEGER m,i,j

        pi=acos(-1.0)
        tol=1.E-12

        m=(n+1)/2
        xm=0.5*(x2+x1)
        xl=0.5*(x2-x1)

        DO 10 i=1,m
         z=cos(pi*(i-0.25)/(N+0.5))

 20      CONTINUE
         p1=1.0E0
         p2=0.0E0
         DO 30 j=1,N
          p3=p2
          p2=p1
          p1=((2*j-1)*z*p2-(j-1)*p3)/j
 30      CONTINUE
         pp=N*(z*p1-p2)/(z*z-1.0E0)
         z1=z
         z=z1-p1/pp
         IF( abs(z1-z) .GT. tol) GOTO 20 ! Scheifenende

         X(i) = xm - xl*z
         X(n+1-i) = xm + xl*z
         W(i) = 2.E0*xl/((1.0-z*z)*pp*pp)
         W(n+1-i) = W(i)
 10     CONTINUE
       END SUBROUTINE gauleg


SUBROUTINE K_Poten2TrigLinear(eNum)
   USE PrecTypes
   USE GlobalData
   IMPLICIT NONE

   ! -------------------------------------------------------------------------
   ! NAME:   K_Poten2TrigLinear
   ! PARENT: Mat_Poten2TrigLinear
   ! STATUS: Current
   ! OWNER:  Toshiro Matsumoto
   ! TEXT:   Calculate stiffness matrix a triangular linear element for
   !         two-dimensional steady-state heat conduction problems.
   ! REVISION DATE: 16-Jul-2009
   ! -------------------------------------------------------------------------

   INTEGER, INTENT(IN) :: eNum

   INTEGER :: i,j,k,l

   ! Variables used for integrations of triangular linear elements
   REAL(dp) :: a1, a2, a3, b1, b2, b3, c1, c2, c3, Delta, Delta2
   REAL(dp) :: coef_b1, coef_c1, coef_b2, coef_c2, coef_b3, coef_c3
   REAL(dp) :: a1_b1xg_c1yg, a2_b2xg_c2yg, a3_b3xg_c3yg
   REAL(dp) :: x1, x2, x3, y1, y2, y3
   REAL(dp) :: Dm(2,2)                           ! [D] matrix
   REAL(dp) :: Bm1(2),    Bm2(2),    Bm3(2)      ! [B] matrix
   REAL(dp) :: Dm_Bm1(2), Dm_Bm2(2), Dm_Bm3(2)   ! [D][B]
   REAL(dp) :: kx, ky
   INTEGER  :: p1, p2, p3

   Ktrig_p = 0.0

   kx  = lambda( emtype(eNum), 1 )  ! thermal conductivity in x-direction
   ky  = lambda( emtype(eNum), 2 )  ! thermal conductivity in y-direction

   ! [D] matrix
   !
   !       |  -kx     0   |
   ! [D] = |              |
   !       |   0     -ky  |
   Dm(1,1) = -kx
   Dm(1,2) = 0.0_dp
   Dm(2,1) = 0.0_dp
   Dm(2,2) = -ky

   Bm1 = 0.0
   Bm2 = 0.0
   Bm3 = 0.0

   ! Global node numbers of the element under consideration
   p1 = elm(eNum,1)
   p2 = elm(eNum,2)
   p3 = elm(eNum,3)

   ! Coordinates of node 1
   x1 = x( p1 )
   y1 = y( p1 )

   ! Coordinates of node 2
   x2 = x( p2 )
   y2 = y( p2 )

   ! Coordinates of node 3
   x3 = x( p3 )
   y3 = y( p3 )

   a1 = x2*y3 - x3*y2;   b1 = y2 - y3;   c1 = x3 - x2
   a2 = x3*y1 - x1*y3;   b2 = y3 - y1;   c2 = x1 - x3
   a3 = x1*y2 - x2*y1;   b3 = y1 - y2;   c3 = x2 - x1

   Delta2 = a1 + a2 + a3
   Delta  = Delta2 * 0.5_dp

   !         |  ∂N1/∂x  |
   ! [B1] =  |          |
   !         |  ∂N1/∂y  |
   Bm1(1) = b1
   Bm1(2) = c1

   !         |  ∂N2/∂x  |
   ! [B2] =  |          |
   !         |  ∂N2/∂y  |
   Bm2(1) = b2
   Bm2(2) = c2

   !         |  ∂N3/∂x  |
   ! [B3] =  |          |
   !         |  ∂N3/∂y  |
   Bm3(1) = b3
   Bm3(2) = c3

   Bm1 = Bm1/Delta2
   Bm2 = Bm2/Delta2
   Bm3 = Bm3/Delta2

   Dm_Bm1(1) = Dm(1,1) * Bm1(1) + Dm(1,2) * Bm1(2)
   Dm_Bm1(2) = Dm(2,1) * Bm1(1) + Dm(2,2) * Bm1(2)

   Dm_Bm2(1) = Dm(1,1) * Bm2(1) + Dm(1,2) * Bm2(2)
   Dm_Bm2(2) = Dm(2,1) * Bm2(1) + Dm(2,2) * Bm2(2)

   Dm_Bm3(1) = Dm(1,1) * Bm3(1) + Dm(1,2) * Bm3(2)
   Dm_Bm3(2) = Dm(2,1) * Bm3(1) + Dm(2,2) * Bm3(2)

   ! [K11] = [B1]^T [D] [B1]
   ! [K12] = [B1]^T [D] [B2]
   ! [K13] = [B1]^T [D] [B3]
   Ktrig_p(1,1) = ( Bm1(1) * Dm_Bm1(1) + Bm1(2) * Dm_Bm1(2) ) * Delta
   Ktrig_p(1,2) = ( Bm1(1) * Dm_Bm2(1) + Bm1(2) * Dm_Bm2(2) ) * Delta
   Ktrig_p(1,3) = ( Bm1(1) * Dm_Bm3(1) + Bm1(2) * Dm_Bm3(2) ) * Delta

   ! [K21] = [B2]^T [D] [B1]
   ! [K22] = [B2]^T [D] [B2]
   ! [K23] = [B2]^T [D] [B3]
   Ktrig_p(2,1) = ( Bm2(1) * Dm_Bm1(1) + Bm2(2) * Dm_Bm1(2) ) * Delta
   Ktrig_p(2,2) = ( Bm2(1) * Dm_Bm2(1) + Bm2(2) * Dm_Bm2(2) ) * Delta
   Ktrig_p(2,3) = ( Bm2(1) * Dm_Bm3(1) + Bm2(2) * Dm_Bm3(2) ) * Delta

   ! [K31] = [B3]^T [D] [B1]
   ! [K32] = [B3]^T [D] [B2]
   ! [K33] = [B3]^T [D] [B3]
   Ktrig_p(3,1) = ( Bm3(1) * Dm_Bm1(1) + Bm3(2) * Dm_Bm1(2) ) * Delta
   Ktrig_p(3,2) = ( Bm3(1) * Dm_Bm2(1) + Bm3(2) * Dm_Bm2(2) ) * Delta
   Ktrig_p(3,3) = ( Bm3(1) * Dm_Bm3(1) + Bm3(2) * Dm_Bm3(2) ) * Delta

   Ktrig_p = Ktrig_p * thickness(eNum)

   RETURN
END SUBROUTINE K_Poten2TrigLinear


! =================
! SUBROUTINE INIGAU
! =================

subroutine inigau_plasim(klat,pz0,pzw)        ! pz0 & pzw are (kind=8) reals !!!
implicit none
integer                  :: klat       ! Number of Gaussian latitudes
real (kind=8)            :: pz0(klat)  ! Gaussian abscissas
real (kind=8)            :: pzw(klat)  ! Gaussian weights
integer                  :: jlat       ! Latitudinal loop index
integer                  :: jiter      ! Iteration loop index
integer      , parameter :: NITER = 50 ! Maximum # of iterations
real (kind=8), parameter :: PI    =  3.14159265358979_8
real (kind=8), parameter :: ZEPS  =  1.0e-16 ! Convergence criterion
real (kind=8) :: z0,z1,z2,z3,z4,z5
real (kind=8) :: ql,qld
! Compute Gaussian abscissas & weights

z0 = PI / (2*klat+1)
z1 = 1.0_8 / (klat*klat*8)
z4 = 2.0_8 / (klat*klat)

do jlat = 1 , klat/2
   z2 = z0 * (2*jlat - 0.5_8)
   z2 = cos(z2 + z1 / tan(z2))
   do jiter = 1 , NITER
      z3 = ql(klat,z2) * qld(klat,z2)
      z2 = z2 - z3
      if (abs(z3) < ZEPS) exit ! converged
   enddo ! jiter
   z5 = ql(klat-1,z2) / sqrt(klat - 0.5_8)
   pz0(jlat) = z2
   pzw(jlat) = z4 * (1.0_8 - z2 * z2) / (z5 * z5)
   pz0(klat-jlat+1) = -z2
   pzw(klat-jlat+1) = pzw(jlat)
enddo ! jlat

return
end subroutine inigau_plasim

! ===========
! FUNCTION QL
! ===========

real (kind=8) function ql(k,p)
implicit none
integer      , intent(IN) :: k
real (kind=8), intent(IN) :: p
real (kind=8) :: z0,z1,z2,z3,z4
integer :: j
z0 = acos(p)
z1 = 1.0
z2 = 0.0
do j = k , 0 , -2
   z3 = z1 * cos(z0 * j)
   z2 = z2 + z3
   z4 = (k-j+1) * (k+j) * 0.5_8
   z1 = z1 * z4 / (z4 + (j-1))
enddo ! j
if (mod(k,2) == 0) z2 = z2 - 0.5_8 * z3

z0 = sqrt(2.0_8)
do j = 1 ,k
   z0 = z0 * sqrt(1.0_8 - 0.25_8 / (j*j))
enddo ! j
ql = z0 * z2
return
end function ql

! ============
! FUNCTION QLD
! ============

real (kind=8) function qld(k,p)
implicit none
integer      , intent(IN) :: k
real (kind=8), intent(IN) :: p
real (kind=8) :: z
real (kind=8) :: ql

z = p * ql(k,p) - sqrt((k + k + 1.0_8) / (k + k - 1.0_8)) * ql(k-1,p)
qld = (p * p - 1.0_8) / (k * z)

return
end function qld


c=======================================================================
c
c     SEXPO
c
c     Generate Standard Exponential distribution
c
c-----------------------------------------------------------------------
c
c     Copyright (c) 2014 NumX
c     All rights reserved.
c
c     This software is the confidential and proprietary information
c     of NumX. You shall not disclose such Confidential
C     Information and shall use it only in accordance with the terms
c     of the licence agreement you entered into with NumX.
c
c     author: Yann Vernaz
c
c-----------------------------------------------------------------------
      DOUBLE PRECISION FUNCTION sexpo()
c-----------------------------------------------------------------------
c
c     Method - for details see:
c
c     AHRENS, J.H. AND DIETER, U.
c     COMPUTER METHODS FOR SAMPLING FROM THE
c     EXPONENTIAL AND NORMAL DISTRIBUTIONS.
c     COMM. ACM, 15,10 (OCT. 1972), 873 - 882.
c
c     ALL STATEMENT NUMBERS CORRESPOND TO THE STEPS OF ALGORITHM
c     'SA' IN THE ABOVE PAPER (SLIGHTLY MODIFIED IMPLEMENTATION)
c
c     Q(N) = SUM(ALOG(2.0)**K/K!)    K=1,..,N ,      THE HIGHEST N
c     (HERE 8) IS DETERMINED BY Q(N)=1.0 WITHIN STANDARD PRECISION
c
c-----------------------------------------------------------------------
c
c     local scalars
      DOUBLE PRECISION a, q1, u, umin, ustar
      INTEGER i
c
c     local arrays
      DOUBLE PRECISION q(8)
c
c     external functions
      DOUBLE PRECISION ranf
      EXTERNAL ranf
c
c     equivalences
      EQUIVALENCE (q(1), q1)
c
c     save statement
      SAVE q
c
c     data statements
      DATA q/.6931472,.9333737,.9888778,.9984959,.9998293,.9999833,
     +     .9999986,.9999999/
c
   10 a = 0.0
      u = ranf()
      GO TO 30

   20 a = a + q1
   30 u = u + u
c     changed the following to reflect the true algorithm and
c     prevent unpredictable behavior if U is initially 0.5.
c      IF (u.LE.1.0) GO TO 20
      IF (u.LT.1.0) GO TO 20
   40 u = u - 1.0
      IF (u.GT.q1) GO TO 60
   50 sexpo = a + u
      RETURN
c
   60 i = 1
      ustar = ranf()
      umin = ustar
   70 ustar = ranf()
      IF (ustar.LT.umin) umin = ustar
   80 i = i + 1
      IF (u.GT.q(i)) GO TO 70
   90 sexpo = a + umin*q1
      RETURN
      END


      subroutine resid_betakd(n_K,n_beta,np1_K,np1_beta,x,Nx,eta,ht,hx,m
     &us,myzero,phys_bdy,res)
      implicit none
      integer i
      integer Nx
      real*8 eta
      real*8 ht
      real*8 hx
      real*8 mus
      real*8 myzero
      real*8 n_K(Nx)
      real*8 n_beta(Nx)
      real*8 np1_K(Nx)
      real*8 np1_beta(Nx)
      real*8 x(Nx)
      integer phys_bdy(2)
      real*8 res
      real*8 qb
      res = 0.0D0
      if (phys_bdy(1) .eq. 1) then
      do i=1, 1, 1
      qb = np1_beta(i) + myzero * x(i)
      res = res + qb**2
      end do
      endif
      do i=2, Nx-1, 1
      qb = (-0.1D1 * n_beta(i) + np1_beta(i)) / ht - 0.2500000000000000D
     #0 * mus * (-0.1D1 * np1_K(i - 1) + np1_K(i + 1)) / hx + 0.50000000
     #00000000D0 * eta * np1_beta(i) - 0.2500000000000000D0 * mus * (-0.
     #1D1 * n_K(i - 1) + n_K(i + 1)) / hx + 0.5000000000000000D0 * eta *
     # n_beta(i)
      res = res + qb**2
      end do
      if (phys_bdy(2) .eq. 1) then
      do i=Nx, Nx, 1
      qb = np1_beta(i) + myzero * x(i)
      res = res + qb**2
      end do
      endif
      res = sqrt(res/(1*Nx))
      END


! ======================================================================
! PROGRAM PHASE
! ----------------------------------------------------------------------
      PROGRAM PHASE
      IMPLICIT NONE

!     Parameter Declarations:
!     -----------------------
      REAL, PARAMETER :: PI = 3.1415926535897932384626433832795028841971E0

!     Input Variable and Array Declarations:
!     --------------------------------------
      INTEGER :: NUMBER_OF_GRID_POINTS
      INTEGER :: NUMBER_OF_TIME_STEPS
      INTEGER :: NUMBER_OF_TIME_STEPS_BEFORE_WRITE
      INTEGER :: DISTRIBUTION_FILE
      INTEGER :: START_MOMENTUM
      INTEGER :: END_MOMENTUM

      REAL :: RADIUS
      REAL :: GAMMA_FACTOR
      REAL :: ROTATION_RATE

!     Internal Variable and Array Declarations:
!     -----------------------------------------
      CHARACTER(80) :: BUFFER

      INTEGER :: CURRENT_TIME_STEP
      INTEGER :: CURRENT_MOMENTUM
      INTEGER :: LENGTH
      INTEGER :: INFO
      INTEGER :: M, N

      REAL :: ANGULAR_GRID_SPACING_SIZE
      REAL :: TIME_STEP_SIZE
      REAL :: TIME
      REAL :: X
      REAL :: Y
      REAL :: PHASESHIFT

      REAL, ALLOCATABLE, DIMENSION(:) :: COEFFICIENT

!     Input Variable Assignment Read from Command-line Arguments:
!     -----------------------------------------------------------
      CALL GET_COMMAND_ARGUMENT(1,BUFFER,LENGTH,INFO)
      READ(BUFFER,*) NUMBER_OF_GRID_POINTS

      CALL GET_COMMAND_ARGUMENT(2,BUFFER,LENGTH,INFO)
      READ(BUFFER,*) NUMBER_OF_TIME_STEPS

      CALL GET_COMMAND_ARGUMENT(3,BUFFER,LENGTH,INFO)
      READ(BUFFER,*) NUMBER_OF_TIME_STEPS_BEFORE_WRITE

      CALL GET_COMMAND_ARGUMENT(4,BUFFER,LENGTH,INFO)
      READ(BUFFER,*) DISTRIBUTION_FILE

      CALL GET_COMMAND_ARGUMENT(5,BUFFER,LENGTH,INFO)
      READ(BUFFER,*) START_MOMENTUM

      CALL GET_COMMAND_ARGUMENT(6,BUFFER,LENGTH,INFO)
      READ(BUFFER,*) END_MOMENTUM

      CALL GET_COMMAND_ARGUMENT(7,BUFFER,LENGTH,INFO)
      READ(BUFFER,*) RADIUS

      CALL GET_COMMAND_ARGUMENT(8,BUFFER,LENGTH,INFO)
      READ(BUFFER,*) GAMMA_FACTOR

      CALL GET_COMMAND_ARGUMENT(9,BUFFER,LENGTH,INFO)
      READ(BUFFER,*) ROTATION_RATE

!     Internal Variable Assignment:
!     -----------------------------
      ANGULAR_GRID_SPACING_SIZE = 2.0E0*PI/FLOAT(NUMBER_OF_GRID_POINTS)
      TIME_STEP_SIZE = (RADIUS*ANGULAR_GRID_SPACING_SIZE)**2/(2.0E0*GAMMA_FACTOR)

!     Allocate Arrays:
!     ----------------
      ALLOCATE(COEFFICIENT(START_MOMENTUM:END_MOMENTUM))

!     Main Program:
!     -------------
      OPEN(UNIT=DISTRIBUTION_FILE,ACTION='READ',FORM='FORMATTED',STATUS='OLD')
      DO CURRENT_MOMENTUM = START_MOMENTUM, END_MOMENTUM
         READ(DISTRIBUTION_FILE,*) INFO, COEFFICIENT(CURRENT_MOMENTUM), ANGULAR_GRID_SPACING_SIZE, ANGULAR_GRID_SPACING_SIZE
      ENDDO
      CLOSE(UNIT=DISTRIBUTION_FILE,STATUS='KEEP')
      COEFFICIENT = SQRT(COEFFICIENT)
      DO CURRENT_TIME_STEP = 1, NUMBER_OF_TIME_STEPS, NUMBER_OF_TIME_STEPS_BEFORE_WRITE
         X = 0.0E0
         Y = 0.0E0
         TIME = FLOAT(CURRENT_TIME_STEP)*TIME_STEP_SIZE
         DO M = START_MOMENTUM+20, END_MOMENTUM
            N = M - 2*10
            X = X + COEFFICIENT(M)*COEFFICIENT(N)*COS(((FLOAT(N)**2-FLOAT(M)**2)/(2*RADIUS**2)+(FLOAT(M)-FLOAT(N))*ROTATION_RATE)*TIME)
            Y = Y - COEFFICIENT(M)*COEFFICIENT(N)*SIN(((FLOAT(N)**2-FLOAT(M)**2)/(2*RADIUS**2)+(FLOAT(M)-FLOAT(N))*ROTATION_RATE)*TIME)
         ENDDO
         PHASESHIFT = ATAN2(Y,X)
         WRITE(6,*) TIME, PHASESHIFT
      ENDDO

!     Deallocate Arrays:
!     ------------------
      DEALLOCATE(COEFFICIENT)

      STOP
      ENDPROGRAM PHASE
! ======================================================================


cc Copyright (C) 2004-2009: Leslie Greengard and June-Yub Lee
cc Contact: greengard@cims.nyu.edu
cc
cc This software is being released under a FreeBSD license
cc (see license.txt in this directory).
c
c tweaked Alex Barnett to call FINUFFT 2/17/17
c dyn malloc; type 2 uses same input data fk0, 3/8/17
c
c Compile with (multithreaded version):
c gfortran nufft1d_demo.f dirft1d.f -o nufft1d_demo ../lib/libfinufft.a
c          -lstdc++ -lfftw3 -lfftw3_omp -lm -fopenmp
c
      program nufft1d_demo
      implicit none
c
c --- local variables
c
      integer i,ier,iflag,j,k1,mx,ms,nj
      real*8, allocatable :: xj(:),sk(:)
      real*8 err,eps,pi
      parameter (pi=3.141592653589793238462643383279502884197d0)
      complex*16, allocatable :: cj(:),cj0(:),cj1(:),fk0(:),fk1(:)
c
c     --------------------------------------------------
c     create some test data
c     --------------------------------------------------
      ms = 90
      nj = 128
c     first alloc everything
      allocate(fk0(ms))
      allocate(fk1(ms))
      allocate(sk(ms))
      allocate(xj(nj))
      allocate(cj(nj))
      allocate(cj0(nj))
      allocate(cj1(nj))
      do k1 = -nj/2, (nj-1)/2
         j = k1+nj/2+1
         xj(j) = pi * dcos(-pi*j/nj)
         cj(j) = dcmplx( dsin(pi*j/nj), dcos(pi*j/nj))
      enddo
c
c     --------------------------------------------------
c     start tests
c     --------------------------------------------------
c
      iflag = 1
      print*,' Start 1D testing: ', ' nj =',nj, ' ms =',ms
      do i = 1,4
         if (i.eq.1) eps=1d-4
         if (i.eq.2) eps=1d-8
         if (i.eq.3) eps=1d-12
         if (i.eq.4) eps=1d-16
c extended/quad precision tests
         if (i.eq.5) eps=1d-20
         if (i.eq.6) eps=1d-24
         if (i.eq.7) eps=1d-28
         if (i.eq.8) eps=1d-32
	 print*,' '
  	 print*,' Requested precision eps =',eps
	 print*,' '
c
c     -----------------------
c     call 1D Type1 method
c     -----------------------
c
         call dirft1d1(nj,xj,cj,iflag, ms,fk0)
         call finufft1d1_f(nj,xj,cj,iflag,eps, ms,fk1,ier)
         call errcomp(fk0,fk1,ms,err)
         print *,' ier = ',ier
         print *,' type 1 error = ',err
c
c     -----------------------
c     call 1D Type2 method
c     -----------------------
c
         call dirft1d2(nj,xj,cj0,iflag, ms,fk0,ier)
         call finufft1d2_f(nj,xj,cj1,iflag, eps, ms,fk0,ier)
         call errcomp(cj0,cj1,nj,err)
         print *,' ier = ',ier
         print *,' type 2 error = ',err
c
c     -----------------------
c     call 1D Type3 method
c     -----------------------
         do k1 = 1, ms
            sk(k1) = 48*dcos(k1*pi/ms)
         enddo
         call dirft1d3(nj,xj,cj,iflag, ms,sk,fk0)
         call finufft1d3_f(nj,xj,cj,iflag,eps, ms,sk,fk1,ier)
         call errcomp(cj0,cj1,nj,err)
         print *,' ier = ',ier
         print *,' type 3 error = ',err
      enddo
      stop
      end
c
c
c
c
c
      subroutine errcomp(fk0,fk1,n,err)
      implicit none
      integer k,n
      complex*16 fk0(n), fk1(n)
      real *8 salg,ealg,err
c
      ealg = 0d0
      salg = 0d0
      do k = 1, n
         ealg = ealg + cdabs(fk1(k)-fk0(k))**2
         salg = salg + cdabs(fk0(k))**2
      enddo
      err =sqrt(ealg/salg)
      return
      end


PROGRAM fizzbuzz
        IMPLICIT NONE ! 暗黙の型宣言を禁止 --- (*1)
        INTEGER :: i  ! 変数iの宣言 --- (*2)
        ! 1から100まで繰り返す --- (*3)
        DO i = 1, 100
                ! 順次判定していく --- (*4)
                IF (MOD(i, 3) == 0 .AND. MOD(i, 5) == 0) THEN
                        PRINT *, "FizzBuzz"
                ELSE IF (MOD(i, 3) == 0) THEN
                        PRINT *, "Fizz"
                ELSE IF (MOD(i, 5) == 0) THEN
                        PRINT *, "Buzz"
                ELSE
                        PRINT *, i
                END IF
        END DO
END PROGRAM fizzbuzz


! test_pointsets.f90 --
!     Test the module for generating sets of points in 1, 2 or 3
!     dimensions
!
program test_pointsets
    use pointsets
    implicit none

    real, dimension(20)   :: x
    real, dimension(1000) :: x2d
    real, dimension(1000) :: y2d
    real                  :: ratio
    real                  :: xb
    real                  :: xe
    real                  :: yb
    real                  :: ye
    integer               :: i

!
! One-dimensional:
! - Uniform spacing
! - Spacing based on geometric series
! - Random spacing
!
    write(*,*) 'Uniform spacing:'
    call arithmetic_spacing( x, 1.0, 20.0 )
    write(*,'(5f10.4)') x
    if ( differ( x(1), 1.0 ) ) then
        write(*,*) '   Too large a difference at start'
    endif
    if ( differ( x(20), 20.0 ) ) then
        write(*,*) '   Too large a difference at end'
    endif

    write(*,*) 'Geometric spacing:'
    call geometric_spacing( x, 1.0, 20.0, 0.5 )
    write(*,'(5f10.4)') x
    if ( differ( x(1), 1.0 ) ) then
        write(*,*) '   Too large a difference at start'
    endif
    if ( differ( x(20), 20.0 ) ) then
        write(*,*) '   Too large a difference at end'
    endif
    ratio = (x(4)-x(3))/(x(3)-x(2))
    if ( differ( ratio, 0.5 ) ) then
        write(*,*) '   Ratio is incorrect'
    endif

    write(*,*) 'Random spacing:'
    call random_spacing( x, 1.0, 20.0 )
    write(*,'(5f10.4)') x
    if ( differ( x(1), 1.0 ) ) then
        write(*,*) '   Too large a difference at start'
    endif
    if ( differ( x(20), 20.0 ) ) then
        write(*,*) '   Too large a difference at end'
    endif

    if ( any( x(2:) < x(1:size(x)-1) ) ) then
        write(*,*) '   Coordinates not in ascending order'
    endif

!
! Two-dimensional case:
! TODO: We ought to test the uniformity ...
!
    write(*,*) 'Two-dimensional random points'
    call random_rectangle( x2d, y2d, 1.0, 10.0 )
    if ( any( x2d < 0.0 ) .or. any( y2d < 0.0 ) ) then
        write(*,*) '    Negative coordinates found'
    endif
    if ( any( x2d > 1.0 ) .or. any( y2d > 10.0 ) ) then
        write(*,*) '    Coordinates out of range'
    endif

    do i = 1,10
        yb = i-1
        ye = i
        write(*,'(2f10.4,i6)') yb, ye, &
            count( y2d >= yb .and. y2d <= ye )
    enddo

    do i = 1,10
        xb = 0.1*(i-1)
        xe = 0.1*i
        write(*,'(2f10.4,i6)') xb, xe, &
            count( x2d >= xb .and. x2d <= xe )
    enddo

contains
logical function differ( x, y )
    real :: x, y

    differ = abs(x-y) > 0.1e-5
end function differ
end program


! Factorials using do loop

program factorial
implicit none

   ! define variables
   integer :: nfact = 1
   integer :: n

   ! compute factorials
   do n = 1, 10
      nfact = nfact * n
      ! print values
      print*,  n, " ", nfact
   end do

end program factorial


      SUBROUTINE DE01OD( CONV, N, A, B, INFO )
C
C     SLICOT RELEASE 5.5.
C
C     Copyright (c) 2002-2012 NICONET e.V.
C
C     PURPOSE
C
C     To compute the convolution or deconvolution of two real signals
C     A and B.
C
C     ARGUMENTS
C
C     Mode Parameters
C
C     CONV    CHARACTER*1
C             Indicates whether convolution or deconvolution is to be
C             performed as follows:
C             = 'C':  Convolution;
C             = 'D':  Deconvolution.
C
C     Input/Output Parameters
C
C     N       (input) INTEGER
C             The number of samples.  N must be a power of 2.  N >= 2.
C
C     A       (input/output) DOUBLE PRECISION array, dimension (N)
C             On entry, this array must contain the first signal.
C             On exit, this array contains the convolution (if
C             CONV = 'C') or deconvolution (if CONV = 'D') of the two
C             signals.
C
C     B       (input) DOUBLE PRECISION array, dimension (N)
C             On entry, this array must contain the second signal.
C             NOTE that this array is overwritten.
C
C     Error Indicator
C
C     INFO    INTEGER
C             = 0:  successful exit;
C             < 0:  if INFO = -i, the i-th argument had an illegal
C                   value.
C
C     METHOD
C
C     This routine computes the convolution or deconvolution of two real
C     signals A and B using an FFT algorithm (SLICOT Library routine
C     DG01MD).
C
C     REFERENCES
C
C     [1] Rabiner, L.R. and Rader, C.M.
C         Digital Signal Processing.
C         IEEE Press, 1972.
C
C     NUMERICAL ASPECTS
C
C     The algorithm requires 0( N*log(N) ) operations.
C
C     CONTRIBUTOR
C
C     Release 3.0: V. Sima, Katholieke Univ. Leuven, Belgium, Feb. 1997.
C     Supersedes Release 2.0 routine DE01CD by R. Dekeyser, State
C     University of Gent, Belgium.
C
C     REVISIONS
C
C     -
C
C     KEYWORDS
C
C     Convolution, deconvolution, digital signal processing, fast
C     Fourier transform, real signals.
C
C     ******************************************************************
C
C     .. Parameters ..
      DOUBLE PRECISION  ZERO, HALF, ONE
      PARAMETER         ( ZERO = 0.0D0, HALF=0.5D0, ONE = 1.0D0 )
C     .. Scalar Arguments ..
      CHARACTER         CONV
      INTEGER           INFO, N
C     .. Array Arguments ..
      DOUBLE PRECISION  A(*), B(*)
C     .. Local Scalars ..
      LOGICAL           LCONV
      INTEGER           J, KJ, ND2P1
      DOUBLE PRECISION  AC, AS, AST, BC, BS, CI, CR
C     .. External Functions ..
      LOGICAL           LSAME
      EXTERNAL          LSAME
C     .. External Subroutines ..
      EXTERNAL          DG01MD, DLADIV, DSCAL, XERBLA
C     .. Intrinsic Functions ..
      INTRINSIC         ABS, DBLE, MAX, MOD
C     .. Executable Statements ..
C
      INFO = 0
      LCONV = LSAME( CONV, 'C' )
C
C     Test the input scalar arguments.
C
      IF( .NOT.LCONV .AND. .NOT.LSAME( CONV, 'D' ) ) THEN
         INFO = -1
      ELSE
         J = 0
         IF( N.GE.2 ) THEN
            J = N
C           WHILE ( MOD( J, 2 ).EQ.0 ) DO
   10       CONTINUE
            IF ( MOD( J, 2 ).EQ.0 ) THEN
               J = J/2
               GO TO 10
            END IF
C           END WHILE 10
         END IF
         IF ( J.NE.1 ) INFO = -2
      END IF
C
      IF ( INFO.NE.0 ) THEN
C
C        Error return.
C
         CALL XERBLA( 'DE01OD', -INFO )
         RETURN
      END IF
C
C     Fourier transform.
C
      CALL DG01MD( 'Direct', N, A, B, INFO )
C
      IF ( LCONV ) THEN
         AST = A(1)*B(1)
      ELSE
         IF ( B(1).EQ.ZERO ) THEN
            AST = ZERO
         ELSE
            AST = A(1)/B(1)
         END IF
      END IF
C
      ND2P1 = N/2 + 1
      J = ND2P1
C
      DO 20 KJ = ND2P1, N
C
C        Components of the transform of function A.
C
         AC = HALF*( A(J) + A(KJ) )
         AS = HALF*( B(J) - B(KJ) )
C
C        Components of the transform of function B.
C
         BC = HALF*( B(KJ) + B(J) )
         BS = HALF*( A(KJ) - A(J) )
C
C        Deconvolution by complex division if CONV = 'D';
C        Convolution by complex multiplication if CONV = 'C'.
C
         IF ( LCONV ) THEN
            CR = AC*BC - AS*BS
            CI = AS*BC + AC*BS
         ELSE
            IF ( MAX( ABS( BC ), ABS( BS ) ).EQ.ZERO ) THEN
               CR = ZERO
               CI = ZERO
            ELSE
               CALL DLADIV( AC, AS, BC, BS, CR, CI )
            END IF
         END IF
C
         A(J)  =  CR
         B(J)  =  CI
         A(KJ) =  CR
         B(KJ) = -CI
         J = J - 1
   20 CONTINUE
      A(1) = AST
      B(1) = ZERO
C
C     Inverse Fourier transform.
C
      CALL DG01MD( 'Inverse', N, A, B, INFO )
C
      CALL DSCAL( N, ONE/DBLE( N ), A, 1 )
C
      RETURN
C *** Last line of DE01OD ***
      END


C$Procedure      RQUAD ( Roots of a quadratic equation )

      SUBROUTINE RQUAD (  A,  B,  C,  ROOT1,  ROOT2  )

C$ Abstract
C
C     Find the roots of a quadratic equation.
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     None.
C
C$ Keywords
C
C     MATH
C     POLYNOMIAL
C     ROOT
C
C$ Declarations

      DOUBLE PRECISION      A
      DOUBLE PRECISION      B
      DOUBLE PRECISION      C
      DOUBLE PRECISION      ROOT1 ( 2 )
      DOUBLE PRECISION      ROOT2 ( 2 )

C$ Brief_I/O
C
C     Variable  I/O  Description
C     --------  ---  --------------------------------------------------
C
C     A          I   Coefficient of quadratic term.
C     B          I   Coefficient of linear term.
C     C          I   Constant.
C     ROOT1      O   Root built from positive discriminant term.
C     ROOT2      O   Root built from negative discriminant term.
C
C$ Detailed_Input
C
C     A,
C     B,
C     C              are the coefficients of a quadratic polynomial
C
C                         2
C                       Ax   +   Bx   +   C.
C
C$ Detailed_Output
C
C     ROOT1,
C     ROOT2         are the roots of the equation,
C
C                         2
C                       Ax   +   Bx   +   C   =  0.
C
C
C                   ROOT1 and ROOT2 are both arrays of length 2.  The
C                   first element of each array is the real part of a
C                   root; the second element contains the complex part
C                   of the same root.
C
C                   When A is non-zero, ROOT1 represents the root
C
C                                    _____________
C                                   /  2
C                      - B   +    \/  B    -   4AC
C                      ---------------------------
C                                    2A
C
C
C                   and ROOT2 represents the root
C
C                                    _____________
C                                   /  2
C                      - B   -    \/  B    -   4AC
C                      --------------------------- .
C                                    2A
C
C
C                   When A is zero and B is non-zero, ROOT1 and ROOT2
C                   both represent the root
C
C                      - C / B.
C
C$ Parameters
C
C     None.
C
C$ Exceptions
C
C     1)   If the input coefficients A and B are both zero, the error
C          SPICE(DEGENERATECASE) is signalled.  The output arguments
C          are not modified.
C
C$ Files
C
C     None.
C
C$ Particulars
C
C     None.
C
C$ Examples
C
C     1)   Humor us and suppose we want to compute the "golden ratio."
C
C          The quantity r is defined by the equation
C
C             1/r = r/(1-r),
C
C          which is equivalent to
C
C              2
C             r   +  r  -  1  =  0.
C
C          The following code frament does the job.
C
C
C             C
C             C     Compute "golden ratio."  The root we want,
C             C
C             C                ___
C             C               /
C             C        -1 + \/  5
C             C        -----------,
C             C             2
C             C
C             C
C             C     is contained in ROOT1.
C             C
C
C                   CALL RQUAD ( 1.D0, 1.D0, -1.D0, ROOT1, ROOT2 )
C
C                   PRINT *, 'The "golden ratio" is ', ROOT1(1)
C
C
C     2)   The equation,
C
C              2
C             x   +  1  =  0
C
C          can be solved by the code fragment
C
C
C             C
C             C     Let's do one with imaginary roots just for fun.
C             C
C
C                   CALL RQUAD ( 1.D0,  0.D0,  1.D0,  ROOT1,  ROOT2 )
C
C                   PRINT *, 'ROOT1 is ', ROOT1
C                   PRINT *, 'ROOT2 is ', ROOT2
C
C          The printed results will be something like:
C
C
C             ROOT1 is 0.000000000000000    1.000000000000000
C             ROOT2 is 0.000000000000000   -1.000000000000000
C
C$ Restrictions
C
C     No checks for overflow of the roots are performed.
C
C$ Literature_References
C
C     None.
C
C$ Author_and_Institution
C
C     N.J. Bachman   (JPL)
C
C$ Version
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, 10-JUL-1990 (NJB)
C
C-&

C$ Index_Entries
C
C     roots of a quadratic equation
C
C-&

C
C     SPICELIB functions
C
      LOGICAL               RETURN

C
C     Local variables
C
      DOUBLE PRECISION      CON
      DOUBLE PRECISION      DISCRM
      DOUBLE PRECISION      LIN
      DOUBLE PRECISION      SCALE
      DOUBLE PRECISION      SQR

      LOGICAL               ZEROED

C
C     Standard SPICE error handling.
C
      IF ( RETURN () ) THEN
         RETURN
      ELSE
         CALL CHKIN ( 'RQUAD' )
      END IF

C
C     The degree of the equation is zero unless at least one of the
C     second or first degree coefficients is non-zero.
C
      IF (  ( A .EQ. 0.D0 )  .AND.  ( B .EQ. 0.D0 )  )  THEN

         CALL SETMSG ( 'Both 1st and 2nd degree coefficients are zero.')
         CALL SIGERR ( 'SPICE(DEGENERATECASE)'                         )
         CALL CHKOUT ( 'RQUAD'                                         )
         RETURN

      END IF

C
C     If we can scale the coefficients without zeroing any of them out,
C     we will do so, to help prevent overflow.
C
      SCALE  = MAX ( ABS(A), ABS(B), ABS(C) )

      ZEROED =      (  ( A .NE. 0.D0 ) .AND. ( A / SCALE .EQ. 0.D0 )  )
     .         .OR. (  ( B .NE. 0.D0 ) .AND. ( B / SCALE .EQ. 0.D0 )  )
     .         .OR. (  ( C .NE. 0.D0 ) .AND. ( C / SCALE .EQ. 0.D0 )  )

      IF ( .NOT. ZEROED ) THEN

         SQR = A / SCALE
         LIN = B / SCALE
         CON = C / SCALE

      ELSE

         SQR = A
         LIN = B
         CON = C

      END IF

C
C     If the second-degree coefficient is non-zero, we have a bona fide
C     quadratic equation, as opposed to a linear equation.
C
      IF ( SQR .NE. 0.D0 ) THEN
C
C        Compute the discriminant.
C
         DISCRM = LIN**2  -  4.D0 * SQR * CON

C
C        A non-negative discriminant indicates that the roots are
C        real.
C
         IF ( DISCRM .GE. 0.D0 ) THEN
C
C           The imaginary parts of both roots are zero.
C
            ROOT1(2) = 0.D0
            ROOT2(2) = 0.D0
C
C           We can take advantage of the fact that CON/SQR is the
C           product of the roots to improve the accuracy of the root
C           having the smaller magnitude.  We compute the larger root
C           first and then divide CON/SQR by it to obtain the smaller
C           root.
C
            IF ( LIN .LT. 0.D0 ) THEN
C
C              ROOT1 will contain the root of larger magnitude.
C
               ROOT1(1) =  ( - LIN + DSQRT(DISCRM) )  /  ( 2.D0 * SQR )

               ROOT2(1) =  ( CON / SQR )  /   ROOT1(1)

            ELSE IF ( LIN .GT. 0.D0 ) THEN
C
C              ROOT2 will contain the root of larger magnitude.
C
               ROOT2(1) =  ( - LIN - DSQRT(DISCRM) )  /  ( 2.D0 * SQR )

               ROOT1(1) =  ( CON / SQR )  /   ROOT2(1)

            ELSE
C
C              The roots have the same magnitude.
C
               ROOT1(1)  =    DSQRT( DISCRM )  /  ( 2.D0 * SQR )

               ROOT2(1)  =  - ROOT1(1)

            END IF

C
C        The only other possibility is that the roots are complex.
C
         ELSE
C
C           The roots are complex conjugates, so they have equal
C           magnitudes.
C
            ROOT1(1)  =  -LIN                /   ( 2.D0 * SQR )
            ROOT1(2)  =   DSQRT( -DISCRM )   /   ( 2.D0 * SQR )

            ROOT2(1)  =   ROOT1(1)
            ROOT2(2)  =  -ROOT1(2)

         END IF

C
C     If the second-degree coefficient is zero, we actually have a
C     linear equation.
C
      ELSE IF ( LIN .NE. 0.D0 ) THEN

         ROOT1(1) =  - CON / LIN
         ROOT1(2) =    0.D0

C
C        We set the second root equal to the first, rather than
C        leaving it undefined.
C
         CALL MOVED ( ROOT1, 2, ROOT2 )

      END IF

      CALL CHKOUT ( 'RQUAD' )
      RETURN
      END