************************************************************************
* Calculate stopping criterion for adaptive time stepping
*
* Based on the configuration, CRITAD will compute a bound EPSAD
* that can be used in the adaptive time stepping as stopping criterion.
*
* In:
*   TIMEIN  - Length of startup phase
*   TIMENS  - Current simulation time
*   TIMEST  - Initial simulation time
*   EPSADI  - error limit during startup phase
*   EPSADL  - standard error limit after startup phase
*   IADIN   - Identifier for the error control in the startup phase
*             =0: use EPSADI during startup phase
*             =1: use linear blending from EPSADI to EPSADL during
*                 startup phase
*             =2: use logarithmic blending from EPSADI to EPSADL during
*                 startup phase
*
* Out:
*   EPSAD   - Stopping criterion for adaptive time step control.
*             During the startup phase (time T in the range
*             TIMEST..TIMEST+TIMEIN), the stopping criterion
*             will be calculated according to IADIN. After the
*             startup phase, EPSADL will be used.
************************************************************************

      SUBROUTINE CRITAD(TIMEIN,TIMENS,TIMEST,EPSADI,EPSADL,EPSAD,IADIN)

      IMPLICIT NONE

      DOUBLE PRECISION PI
      PARAMETER (PI=3.1415926535897931D0)

C parameters

      DOUBLE PRECISION TIMEIN,TIMENS,TIMEST,EPSADI,EPSADL,EPSAD
      INTEGER IADIN

C local variables

      DOUBLE PRECISION TDIFF

C     Standard result: EPSADL

      EPSAD=EPSADL

C     Do we have a startup phase at all? Would be better, otherwise
C     Navier-Stokes might stagnate with decreasing time steps at the
C     beginning...

      IF (TIMEIN.GT.0) THEN

C       Calculate the "current position" of the simulation time in
C       the interval TIMEST..TIMEST+TIMEIN

        TDIFF=TIMENS-TIMEST

C       Standard error control in the startup phase

        IF (IADIN.EQ.0) THEN

C         Use EPSADI during startup phase and EPSADL afterwards:

          IF (TDIFF.LE.TIMEIN) THEN
            EPSAD=EPSADI
          ELSE
            EPSAD=EPSADL
          ENDIF

        ENDIF

C       Linear blending as control in the startup phase.
C       Blend linearly between EPSADI and EPSADL from the initial
C       simulation time TIMNEST to the end of the startup phase
C       TIMEST+TIMEIN.
C       After the startup phase, use EPSADL.

        IF (IADIN.EQ.1) THEN
          IF (TDIFF.LE.TIMEIN) THEN
            EPSAD=EPSADI+TDIFF/TIMEIN*(EPSADL-EPSADI)
          ELSE
            EPSAD=EPSADL
          ENDIF
        ENDIF

C       Logarithmic blending as control in the startup phase.
C       Blend logarithmically between EPSADI and EPSADL from the initial
C       simulation time TIMNEST to the end of the startup phase
C       TIMEST+TIMEIN.
C       After the startup phase, use EPSADL.

        IF (IADIN.EQ.2) THEN
          IF (TDIFF.LE.TIMEIN) THEN
            EPSAD=EPSADI**(1D0-TDIFF/TIMEIN)*EPSADL**(TDIFF/TIMEIN)
          ELSE
            EPSAD=EPSADL
          ENDIF
        ENDIF

      ENDIF

      END


C$Procedure      INCNSG ( Intersection of cone and line segment )

      SUBROUTINE INCNSG ( APEX,   AXIS,  ANGLE, ENDPT1,
     .                    ENDPT2, NXPTS, XPT1,  XPT2   )

C$ Abstract
C
C     Compute the points of intersection of a specified nappe of a cone
C     and a line segment.
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     CONE
C     GEOMETRY
C     INTERSECTION
C     LINE
C     MATH
C     SEGMENT
C
C$ Declarations

      IMPLICIT NONE

      DOUBLE PRECISION      APEX   ( 3 )
      DOUBLE PRECISION      AXIS   ( 3 )
      DOUBLE PRECISION      ANGLE
      DOUBLE PRECISION      ENDPT1 ( 3 )
      DOUBLE PRECISION      ENDPT2 ( 3 )
      INTEGER               NXPTS
      DOUBLE PRECISION      XPT1   ( 3 )
      DOUBLE PRECISION      XPT2   ( 3 )

C$ Brief_I/O
C
C     Variable  I/O  Description
C     --------  ---  --------------------------------------------------
C     APEX       I   Apex of cone.
C     AXIS       I   Axis of cone.
C     ANGLE      I   Angle of cone.
C     ENDPT1,
C     ENDPT2     I   Endpoints of line segment.
C     NXPTS      O   Number of intersection points.
C     XPT1       O   First intersection point, if it exists.
C     XPT2       O   Second intersection point, if it exists.
C
C$ Detailed_Input
C
C     APEX       is the apex (tip) of the cone. In this routine's
C                documentation, we'll consider the cone to be a
C                semi-infinite pyramid with circular cross-section. In
C                some contexts, this object is called one "nappe" of
C                the complete cone.
C
C     AXIS       is an axis vector of the cone.
C
C     ANGLE      is the angular separation from AXIS of the rays
C                comprising the cone. Let the notation
C
C                   < A, B >
C
C                denote the dot product of vectors A and B, and let
C
C                   ||A||
C
C                denote the norm of vector A. Then the cone is the set
C                of points
C
C                             X-APEX       AXIS
C                   { X:  < ----------,  -------- >  =  cos(ANGLE) }
C                           ||X-APEX||   ||AXIS||
C
C
C     ENDPT1,
C     ENDPT2     are endpoints of a line segment. These points
C                must be distinct.
C
C$ Detailed_Output
C
C     NXPTS      is the number of points of intersection of the input
C                line segment and cone.
C
C     XPT1       is the point of intersection of the segment and cone
C                that is closest to ENDPT1, if an intersection exists.
C                If there are no intersections, XPT1 is undefined.
C
C     XPT2       is the point of intersection of the segment and cone
C                that is farthest from ENDPT1, if two points of
C                intersection exist. If there are not two
C                intersections, XPT2 is undefined.
C
C$ Parameters
C
C     None.
C
C$ Exceptions
C
C     1)  If AXIS is the zero vector, the error SPICE(ZEROVECTOR)
C         will be signaled.
C
C     2)  If ANGLE is less than zero, the error SPICE(INVALIDANGLE)
C         will be signaled.
C
C     3)  If ENDPT1 and ENDPT2 coincide, the error
C         SPICE(ENDPOINTSMATCH) will be signaled.
C
C$ Files
C
C     None.
C
C$ Particulars
C
C     This routine is used by the SPICELIB DSK subsystem. In
C     particular, it is used to determine whether a ray contacts a
C     latitude boundary of a volume element in either planetocentric
C     latitudinal or planetodetic coordinates.
C
C$ Examples
C
C     The numerical results shown for these examples may differ across
C     platforms. The results depend on the SPICE kernels used as input
C     (if any), the compiler and supporting libraries, and the machine
C     specific arithmetic implementation.
C
C     1) Compute the intersection of a line segment and cone in
C        a simple case for which the results can easily be checked.
C
C        Let the apex of the cone be at the origin. Let the axis
C        of the cone lie on the +X axis. Let the angle of the cone
C        be 45 degrees. Let the line segment have endpoints
C
C           ENDPT1 = ( 1,   -2, sqrt(3)/2 )
C           ENDPT2 = ( 1,    2, sqrt(3)/2 )
C
C        We expect there to be two points of intersection:
C
C           XPT1   = ( 1, -1/2, sqrt(3)/2 )
C           XPT2   = ( 1,  1/2, sqrt(3)/2 )
C
C
C        Example code begins here.
C
C
C              PROGRAM EX1
C              IMPLICIT NONE
C        C
C        C     SPICELIB functions
C        C
C              DOUBLE PRECISION      RPD
C        C
C        C     Local parameters
C        C
C              CHARACTER*(*)         FMT1
C              PARAMETER           ( FMT1 = '(A,3F13.8)' )
C
C              CHARACTER*(*)         FMT2
C              PARAMETER           ( FMT2 = '(A,I2)' )
C        C
C        C     Local variables
C        C
C              DOUBLE PRECISION      ANGLE
C              DOUBLE PRECISION      APEX   ( 3 )
C              DOUBLE PRECISION      AXIS   ( 3 )
C              DOUBLE PRECISION      ENDPT1 ( 3 )
C              DOUBLE PRECISION      ENDPT2 ( 3 )
C              DOUBLE PRECISION      SQ3
C              DOUBLE PRECISION      XPT1   ( 3 )
C              DOUBLE PRECISION      XPT2   ( 3 )
C
C              INTEGER               NXPTS
C
C        C
C        C     Set up the cone's geometric attributes.
C        C
C              CALL VPACK ( 0.D0, 0.D0, 0.D0, APEX )
C              CALL VPACK ( 1.D0, 0.D0, 0.D0, AXIS )
C
C              ANGLE = 45.D0 * RPD()
C        C
C        C     Initialize the line segment's endpoints.
C        C
C              SQ3 = SQRT( 3.D0  )
C
C              CALL VPACK ( 1.D0, -2.D0, SQ3/2, ENDPT1 )
C              CALL VPACK ( 1.D0,  2.D0, SQ3/2, ENDPT2 )
C        C
C        C     Find the points of intersection.
C        C
C              CALL INCNSG ( APEX,   AXIS,  ANGLE, ENDPT1,
C             .              ENDPT2, NXPTS, XPT1,  XPT2   )
C
C              WRITE (*,*) ' '
C              WRITE (*,FMT1) 'Apex:        ', APEX
C              WRITE (*,FMT1) 'Axis:        ', AXIS
C              WRITE (*,FMT1) 'Angle (deg): ', ANGLE/RPD()
C              WRITE (*,FMT1) 'Endpoint 1:  ', ENDPT1
C              WRITE (*,FMT1) 'Endpoint 2:  ', ENDPT2
C              WRITE (*,*) ' '
C              WRITE (*,FMT2) 'Number of intersection points: ',
C             .            NXPTS
C              WRITE (*,*) ' '
C              WRITE (*,FMT1) 'Point 1:    ', XPT1
C              WRITE (*,FMT1) 'Point 2:    ', XPT2
C              WRITE (*,*) ' '
C
C              END
C
C
C     When this program was executed on a PC/Linux/gfortran/64-bit
C     platform, the output was:
C
C
C        Apex:           0.00000000   0.00000000   0.00000000
C        Axis:           1.00000000   0.00000000   0.00000000
C        Angle (deg):   45.00000000
C        Endpoint 1:     1.00000000  -2.00000000   0.86602540
C        Endpoint 2:     1.00000000   2.00000000   0.86602540
C
C        Number of intersection points:  2
C
C        Point 1:       1.00000000  -0.50000000   0.86602540
C        Point 2:       1.00000000   0.50000000   0.86602540
C
C
C$ Restrictions
C
C     1)  This routine is designed to avoid arithmetic overflow in
C         normal cases, such as those in which the line segment is
C         nearly parallel to the cone. However, it is possible to cause
C         arithmetic overflow by using input vectors with extremely
C         large magnitudes.
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.0 26-OCT-2016 (NJB)
C
C-&

C$ Index_Entries
C
C     intersection of line segment and cone
C
C-&

C
C     SPICELIB functions
C
      DOUBLE PRECISION      HALFPI
      DOUBLE PRECISION      PI
      DOUBLE PRECISION      VDIST
      DOUBLE PRECISION      VDOT

      LOGICAL               FAILED
      LOGICAL               RETURN

C
C     Local parameters
C
      DOUBLE PRECISION      COSTOL
      PARAMETER           ( COSTOL = 1.D-10 )

      DOUBLE PRECISION      HSPTOL
      PARAMETER           ( HSPTOL = 1.D-14 )

      INTEGER               UBPL
      PARAMETER           ( UBPL   = 4 )

C
C     Local variables
C
      DOUBLE PRECISION      A
      DOUBLE PRECISION      AXMAG
      DOUBLE PRECISION      B
      DOUBLE PRECISION      C
      DOUBLE PRECISION      CA2
      DOUBLE PRECISION      COLAT
      DOUBLE PRECISION      COSANG
      DOUBLE PRECISION      COSERR
      DOUBLE PRECISION      DIR    ( 3 )
      DOUBLE PRECISION      DMAG
      DOUBLE PRECISION      DP1
      DOUBLE PRECISION      DP2
      DOUBLE PRECISION      LOCANG
      DOUBLE PRECISION      MAXLAT
      DOUBLE PRECISION      MAXP   ( 3 )
      DOUBLE PRECISION      MINLAT
      DOUBLE PRECISION      MINP   ( 3 )
      DOUBLE PRECISION      NRMPLN ( UBPL )
      DOUBLE PRECISION      OFF1   ( 3 )
      DOUBLE PRECISION      OFF2   ( 3 )
      DOUBLE PRECISION      ORIGIN ( 3 )
      DOUBLE PRECISION      PLNX   ( 3 )
      DOUBLE PRECISION      S1
      DOUBLE PRECISION      S2
      DOUBLE PRECISION      UAXIS  ( 3 )
      DOUBLE PRECISION      UDIR   ( 3 )
      DOUBLE PRECISION      UOFF1  ( 3 )
      DOUBLE PRECISION      UOFF2  ( 3 )
      DOUBLE PRECISION      UUAX
      DOUBLE PRECISION      UV1    ( 3 )
      DOUBLE PRECISION      UV2    ( 3 )
      DOUBLE PRECISION      V1     ( 3 )
      DOUBLE PRECISION      V1MAG
      DOUBLE PRECISION      V2     ( 3 )
      DOUBLE PRECISION      V2MAG
      DOUBLE PRECISION      VTEMP  ( 3 )
      DOUBLE PRECISION      VTEMP2 ( 3 )
      DOUBLE PRECISION      W2
      DOUBLE PRECISION      WU
      DOUBLE PRECISION      WUAX
      DOUBLE PRECISION      X      ( 3 )
      DOUBLE PRECISION      XOFF1  ( 3 )
      DOUBLE PRECISION      XOFF2  ( 3 )
      DOUBLE PRECISION      XFORM  ( 3, 3 )
      DOUBLE PRECISION      Y      ( 3 )
      DOUBLE PRECISION      Z      ( 3 )

      INTEGER               I
      INTEGER               N
      INTEGER               NPLNX

      LOGICAL               IN1
      LOGICAL               IN2
      LOGICAL               ISBRCK
      LOGICAL               NEG1
      LOGICAL               NEG2

C
C     Saved values
C
      SAVE                  ORIGIN
      SAVE                  Y
      SAVE                  Z

C
C     Initial values
C
      DATA                  ORIGIN / 3 * 0.D0         /
      DATA                  Y      / 0.D0, 1.D0, 0.D0 /
      DATA                  Z      / 0.D0, 0.D0, 1.D0 /

C
C     Use quasi-discovery check-in. We'll check in before
C     code sections that can generate SPICE errors, and check
C     out afterward. When those code sections are skipped,
C     we avoid traceback participation.
C
      IF ( RETURN() ) THEN
         RETURN
      END IF

C
C     No intersection was found so far.
C
      NXPTS = 0

C
C     The cone's axis vector must be non-zero.
C
      CALL UNORM ( AXIS, UAXIS, AXMAG )

      IF ( AXMAG .EQ. 0.D0 ) THEN

         CALL CHKIN  ( 'INCNSG'                                       )
         CALL SETMSG ( 'The cone''s axis vector must be non-zero '
     .   //            'but sadly, it failed to meet this criterion.' )
         CALL SIGERR ( 'SPICE(ZEROVECTOR)'                            )
         CALL CHKOUT ( 'INCNSG'                                       )
         RETURN

      END IF

C
C     The cone's angular radius must be non-negative.
C
      IF ( ANGLE .LT. 0.D0 ) THEN

         CALL CHKIN  ( 'INCNSG'                             )
         CALL SETMSG ( 'The cone''s angular radius must be '
     .   //            ' non-negative but was # (radians).' )
         CALL ERRDP  ( '#', ANGLE                           )
         CALL SIGERR ( 'SPICE(INVALIDANGLE)'                )
         CALL CHKOUT ( 'INCNSG'                             )
         RETURN

      END IF

C
C     The endpoints of the segment must be distinct. Check this after
C     computing a unit direction vector for the line segment.
C
      CALL VSUB  ( ENDPT2, ENDPT1, DIR )

      CALL UNORM ( DIR, UDIR, DMAG )

      IF ( DMAG .EQ. 0.D0 ) THEN

         CALL CHKIN  ( 'INCNSG'                              )
         CALL SETMSG ( 'The distance between the segment''s '
     .   //            'endpoints was zero. First endpoint: '
     .   //            '(# # #).'                            )
         CALL ERRDP  ( '#', ENDPT1(1)                        )
         CALL ERRDP  ( '#', ENDPT1(2)                        )
         CALL ERRDP  ( '#', ENDPT1(3)                        )
         CALL SIGERR ( 'SPICE(ENDPOINTSMATCH)'               )
         CALL CHKOUT ( 'INCNSG'                              )
         RETURN

      END IF

C
C     Store the cosine of the cone's angular radius. We'll treat all
C     cases with COSANG equal to 0 as though the cone is actually a
C     plane normal to the axis and containing the apex.
C
      COSANG = COS( ANGLE )
      LOCANG = ANGLE
C
C     We'll work with a local axis that has angular separation of
C     no more than pi/2 from the nappe.
C
      IF ( COSANG .LT. 0.D0 ) THEN

         COSANG   = - COSANG
         LOCANG   =   PI() - ANGLE

         UAXIS(1) = - UAXIS(1)
         UAXIS(2) = - UAXIS(2)
         UAXIS(3) = - UAXIS(3)

      END IF

C
C     Compute the offsets of the endpoints of the segment from
C     the cone's apex.
C
      CALL VSUB ( ENDPT1, APEX, OFF1 )
      CALL VSUB ( ENDPT2, APEX, OFF2 )

C
C     Deal with some of the simple cases first.
C
      CALL VHAT ( OFF1, UOFF1 )
      CALL VHAT ( OFF2, UOFF2 )

      DP1  = VDOT( UOFF1, UAXIS )
      DP2  = VDOT( UOFF2, UAXIS )

C
C     The given axis is inside the nappe defined by the angular radius.
C
C     There's no intersection if both endpoints are in the interior of
C     the nappe of the cone (since the nappe is convex).
C
      IN1  = DP1 .GE. COSANG
      IN2  = DP2 .GE. COSANG
C
C     If the line segment lies on the far side of the plane that
C     contains the apex and is orthogonal to the axis, there's no
C     intersection.
C
      NEG1 = DP1 .LT. 0.D0
      NEG2 = DP2 .LT. 0.D0

      IF (      ( IN1  .AND. IN2  )
     .     .OR. ( NEG1 .AND. NEG2 ) ) THEN
C
C        The segment is in the interior of the cone or
C        on the far side of the plane.
C
         NXPTS = 0

         RETURN

      END IF

C
C     Here's where we handle the half-space case.
C
      IF ( ABS(COSANG) .LT. HSPTOL ) THEN
C
C        See whether the ray emanating from the first endpoint and
C        having direction UDIR hits the plane normal to the axis and
C        containing the apex. We'll call this plane NRMPLN.
C
C        NVP2PL can signal an error only if the input axis is the
C        zero vector. We've ensured that it isn't.
C
         CALL NVP2PL ( UAXIS,  APEX, NRMPLN )
         CALL INRYPL ( ENDPT1, UDIR, NRMPLN, NPLNX, PLNX )

C
C        If the ray doesn't hit the plane, we're done. Otherwise,
C        check the intercept.
C
         IF ( NPLNX .EQ. 1 ) THEN
C
C           The ray does hit the plane. If the intersection is on the
C           line segment, we have a solution.
C
            IF ( VDIST(PLNX, ENDPT1) .LE. DMAG ) THEN
C
C              The intercept is not further along the ray than the
C              second endpoint. It's a valid solution.
C
               NXPTS = 1
               CALL VEQU ( PLNX, XPT1 )

            END IF

         END IF
C
C        This is the end of the half-space case.
C
         RETURN

      END IF

C
C     At this point we've disposed of the trivial cases. We'll
C     set up a quadratic equation for the intersection of the
C     line segment with the surface of the cone's nappe.
C
C     Due to round-off errors, the solution of the quadratic may
C     either be inaccurate or may not be found at all. We'll
C     examine the solutions we find and solve the problem by
C     an alternate method if necessary. However, the quadratic
C     method is fast, so we give it priority.
C
C     The equation of a ray starting at ENDPT1 and having unit
C     direction vector UDIR is
C
C        RAY  = { ENDPT1 + s*UDIR, s >= 0 }                          (1)
C
C     The equation of the nappe of the cone is
C
C        CONE = { X: < X - APEX, UAXIS > = ||X-APEX|| * cos(ANGLE) } (2)
C
C     where ANGLE is the angular radius of the cone and UAXIS is the
C     unit axis vector. Substituting the right hand side expression of
C     (1) for X in equation (2) and squaring both sides yields a
C     quadratic equation for S. We'll derive the coefficients of the
C     equation below.
C
C     Let
C
C        Q  = X - APEX
C        W  = ENDPT1 - APEX
C        U  = UDIR
C        CA = cos(ANGLE)
C
C     We can translate the cone and ray by -APEX, and (1) and (2)
C     can be re-written as
C
C        RAY  = { W + s*U, s >= 0 }                                  (3)
C
C        CONE = { Q: < Q, UAXIS > = ||Q|| * cos(ANGLE) }             (4)
C
C
C        Substituting the ray expression for Q, we obtain
C
C           < W + s*U, UAXIS > = ||W+s*U|| * CA                      (5)
C
C        and squaring both sides yields
C
C                      2                                      2   2
C             <W,UAXIS>  + 2*<W,UAXIS>*<U,UAXIS>*s + <U,UAXIS> * s
C
C                    2                2       2
C           = ( ||W||  + 2*<W,U>*s + s  ) * CA                       (6)
C
C
C       Collecting coefficients of powers of s, we have
C
C                         2     2     2
C              ( <U,UAXIS>  - CA ) * s
C
C                                            2
C            + 2 * ( <W,UAXIS>*<U,UAXIS> - CA * <W,U> ) * s
C
C                       2        2    2
C            + <W,UAXIS>  - ||W|| * CA
C
C
C         =  0                                                       (7)
C
C
C      Before continuing, we observe that the only non-unit vector
C      in (7) is W. So the coefficients in (7) have no possibility
C      of overflowing unless the vertex of the ray is very far from
C      the apex of the cone.
C
C      W has been computed above as OFF1.
C
C
C         [ Consider adding check on OFF1 here. ]
C
C
C     Intermediate values:
C
      UUAX = VDOT( UDIR, UAXIS )
      WUAX = VDOT( OFF1, UAXIS )
      WU   = VDOT( OFF1, UDIR  )
      W2   = VDOT( OFF1, OFF1  )
      CA2  = COSANG * COSANG

C
C     Quadratic coefficients:
C
      A    = ( UUAX * UUAX ) - CA2

      B    = 2 * (  ( WUAX * UUAX ) - ( CA2 * WU )  )

      C    = ( WUAX * WUAX ) - ( W2 * CA2 )

C
C     We're not interested in solutions that lie outside
C     of the line segment. The length of the segment is
C     DMAG.
C
C     Solve the equation, using DMAG as an upper bound
C     on the magnitude of the roots.
C
      CALL ZZCNQUAD ( A, B, C, DMAG, N, S1, S2 )

C
C     Compute the possible intersection points and test them
C     to make sure they really are solutions.
C
      IF ( N .GT. 0 ) THEN
C
C        Start with the solution closest to the ray's vertex.
C        Compute XPT1 and make sure it's on the correct nappe
C        of the cone.
C
         IF ( S1 .GE. 0.D0 ) THEN

            XPT1(1) = ENDPT1(1) + S1*UDIR(1)
            XPT1(2) = ENDPT1(2) + S1*UDIR(2)
            XPT1(3) = ENDPT1(3) + S1*UDIR(3)

            CALL VSUB  ( XPT1, APEX, V1 )

C
C           See whether V1 is on the cone.
C
            CALL UNORM ( V1, UV1, V1MAG )

            IF ( V1MAG .GT. 0.D0 ) THEN

               COSERR = ABS( VDOT(UV1,UAXIS) - COSANG )
            ELSE
               COSERR = 0.D0
            END IF

            IF (      ( V1MAG  .EQ. 0.D0   )
     .           .OR. ( COSERR .LT. COSTOL ) ) THEN
C
C              The root is on the cone (on the apex if V1MAG is zero).
C
C              We accept this root. Update NXPTS. Note that this is
C              not necessarily the final value of NXPTS; that
C              depends on the validity of the second root.

               NXPTS = 1

            END IF

         END IF

         IF ( N .EQ. 2 ) THEN
C
C           Check the second root.
C
            IF ( S2 .GE. 0.D0 ) THEN

               XPT2(1) = ENDPT1(1) + S2*UDIR(1)
               XPT2(2) = ENDPT1(2) + S2*UDIR(2)
               XPT2(3) = ENDPT1(3) + S2*UDIR(3)

               CALL VSUB  ( XPT2, APEX, V2 )

C
C              See whether V2 is on the cone.
C
               CALL UNORM ( V2, UV2, V2MAG )

               IF ( V2MAG .GT. 0.D0 ) THEN

                  COSERR = ABS( VDOT(UV2,UAXIS) - COSANG )
               ELSE
                  COSERR = 0.D0
               END IF

               IF (      ( V2MAG  .EQ. 0.D0   )
     .              .OR. ( COSERR .LT. COSTOL ) ) THEN
C
C                 The root is on the cone (on the apex if V2MAG is
C                 zero).
C
C                 We accept this root.
C
                  NXPTS = NXPTS + 1

                  IF ( NXPTS .EQ. 1 ) THEN
C
C                    This is the only valid root; overwrite XPT1.
C
                     CALL VEQU ( XPT2, XPT1 )

                  END IF

               END IF

            END IF

         END IF

      END IF

C
C     We're not done yet. If we have fewer roots than we should, we'll
C     need to solve the problem by an alternate method.
C
C     If we have two roots, we're in good shape. Otherwise we must
C     determine how many roots should be found.
C
      IF ( NXPTS .LT. 2 ) THEN
C
C        We must determine the expected number of roots, and if
C        we didn't come up with them, we must find the roots
C        by an alternate method.
C
C        We'll examine the containment of the endpoints within the
C        cone.
C
C        The case where both endpoints are inside the cone was handled
C        earlier.
C
C        If one endpoint is inside the cone and one is outside,
C        we expect to have one root.
C
         IF (     ( IN1 .AND. ( .NOT. IN2 ) )
     .       .OR. ( IN2 .AND. ( .NOT. IN1 ) )  ) THEN
C
C           There's supposed to be one root. If we found none, find one
C           now.
C
            IF ( NXPTS .EQ. 0 ) THEN
C
C              ZZCXBRUT signals an error if the axis is the zero
C              vector, but not otherwise. We've already ruled out this
C              situation. Therefore, we don't check in before the
C              following call.
C
               CALL ZZCXBRUT ( APEX,   UAXIS,  LOCANG,
     .                         ENDPT1, ENDPT2, XPT1,  ISBRCK )

               IF ( ISBRCK ) THEN
C
C                 As long as the root was bracketed, XPT1 is a
C                 solution.
C
                  NXPTS = 1

               END IF

            END IF

         ELSE

            CALL CHKIN ( 'INCNSG' )
C
C           Both endpoints are outside the cone. We could have zero to
C           two roots. If the minimum angular separation of the segment
C           from the axis is less than ANGLE, we expect to find two
C           roots; if it's equal to ANGLE, we expect to find one, and
C           if it's greater than ANGLE, none.
C
C           We'll transform OFF1 and OFF2 into a reference frame in
C           which angular separation from the axis is equivalent to
C           colatitude. Then we'll find the maximum latitude attained
C           on the segment.
C
C           We'll count the roots we find, so we'll start at zero.
C
            NXPTS = 0

            CALL FRAME ( UAXIS, X, Y )

            DO I = 1, 3

               XFORM(1,I) = X(I)
               XFORM(2,I) = Y(I)
               XFORM(3,I) = UAXIS(I)

            END DO

            CALL MXV ( XFORM, OFF1, XOFF1 )
            CALL MXV ( XFORM, OFF2, XOFF2 )

            CALL ZZSGLATX ( XOFF1, XOFF2, MINLAT, MINP, MAXLAT, MAXP )

            IF ( FAILED() ) THEN
               CALL CHKOUT ( 'INCNSG' )
               RETURN
            END IF

C
C           COLAT is the colatitude of the point of maximum latitude.
C
            COLAT = HALFPI() - MAXLAT

            IF ( COLAT .LT. LOCANG ) THEN
C
C              MAXP is inside the cone. There should be an intersection
C              on the segment between XOFF1 and MAXP and another
C              between MAXP and XOFF2.
C
               CALL ZZCXBRUT ( ORIGIN, Z,    LOCANG,
     .                         XOFF1,  MAXP, VTEMP,  ISBRCK )

               IF ( ISBRCK ) THEN
C
C                 Convert VTEMP to the original frame, then translate
C                 it so that it's represented as an offset from the
C                 origin.
C
                  CALL MTXV ( XFORM,  VTEMP, VTEMP2 )
                  CALL VADD ( VTEMP2, APEX,  XPT1   )

                  NXPTS = 1

               END IF

               CALL ZZCXBRUT ( ORIGIN, Z,     LOCANG,
     .                         MAXP,   XOFF2, VTEMP,  ISBRCK )

               IF ( ISBRCK ) THEN
C
C                 Convert VTEMP to the original frame, then translate
C                 it so that it's represented as an offset from the
C                 origin.
C
                  CALL MTXV ( XFORM,  VTEMP, VTEMP2 )
                  CALL VADD ( VTEMP2, APEX,  XPT2   )

                  IF ( NXPTS .EQ. 1 ) THEN
C
C                    Both roots are valid.
C
                     NXPTS = 2

                  ELSE
C
C                    The second root is the only valid root. Move it
C                    into XPT1.
C
                     CALL VEQU ( XPT2, XPT1 )

                     NXPTS = 1

                  END IF

               END IF

            ELSE IF ( COLAT .EQ. LOCANG ) THEN
C
C              The root corresponds to a point of tangency of
C              the segment and cone. This occurs at the point
C              having maximum latitude: MAXP.
C
               CALL VEQU ( MAXP, XPT1 )

               NXPTS = 1

C
C           Note that if COLAT > LOCANG, there are no roots.
C
            END IF

            CALL CHKOUT ( 'INCNSG' )

         END IF
C
C        This is the end of portion of the "brute force" branch in
C        which both endpoints are outside the cone.
C
      END IF

C
C     NXPTS  has been set.
C
C     If NXPTS is 1, then XPT1 is set.
C
C     If NXPTS is 2, then both XPT1 and XPT2 are set.
C

      RETURN
      END


!
! Copyright (C) 2002-2009 Quantum ESPRESSO group
! This file is distributed under the terms of the
! GNU General Public License. See the file `License'
! in the root directory of the present distribution,
! or http://www.gnu.org/copyleft/gpl.txt .
!
!---------------------------------------------------------------------
function qe_erf (x)
  !---------------------------------------------------------------------
  !
  !     Error function - computed from the rational approximations of
  !     W. J. Cody, Math. Comp. 22 (1969), pages 631-637.
  !
  !     for abs(x) le 0.47 erf is calculated directly
  !     for abs(x) gt 0.47 erf is calculated via erf(x)=1-erfc(x)
  !
  IMPLICIT NONE
  INTEGER, PARAMETER :: DP=8
  REAL(DP), INTENT(IN) :: x
  REAL(DP) :: x2, p1 (4), q1 (4)
  REAL(DP), EXTERNAL :: qe_erfc
  REAL(DP) :: qe_erf
  DATA p1 / 2.426679552305318E2_DP, 2.197926161829415E1_DP, &
            6.996383488619136_DP,  -3.560984370181538E-2_DP /
  DATA q1 / 2.150588758698612E2_DP, 9.116490540451490E1_DP, &
            1.508279763040779E1_DP, 1.000000000000000_DP /
  !
  IF (abs (x) > 6.0_DP) THEN
     !
     !  erf(6)=1-10^(-17) cannot be distinguished from 1
     !
     qe_erf = sign (1.0_DP, x)
  ELSE
     IF (abs (x)  <= 0.47_DP) THEN
        x2 = x**2
        qe_erf=x *(p1 (1) + x2 * (p1 (2) + x2 * (p1 (3) + x2 * p1 (4) ) ) ) &
                / (q1 (1) + x2 * (q1 (2) + x2 * (q1 (3) + x2 * q1 (4) ) ) )
     ELSE
        qe_erf = 1.0_DP - qe_erfc (x)
     ENDIF
  ENDIF
  !
  RETURN
END FUNCTION qe_erf
!
!---------------------------------------------------------------------
FUNCTION qe_erfc (x)
  !---------------------------------------------------------------------
  !
  !     erfc(x) = 1-erf(x)  - See comments in erf
  !
  IMPLICIT NONE
  INTEGER, PARAMETER :: DP=8
  REAL(DP), INTENT(IN) :: x
  REAL(DP) :: qe_erfc
  REAL(DP) :: ax, x2, xm2, p2 (8), q2 (8), p3 (5), q3 (5), pim1
  REAL(DP), EXTERNAL :: qe_erf
  DATA p2 / 3.004592610201616E2_DP,  4.519189537118719E2_DP, &
            3.393208167343437E2_DP,  1.529892850469404E2_DP, &
            4.316222722205674E1_DP,  7.211758250883094_DP,   &
            5.641955174789740E-1_DP,-1.368648573827167E-7_DP /
  DATA q2 / 3.004592609569833E2_DP,  7.909509253278980E2_DP, &
            9.313540948506096E2_DP,  6.389802644656312E2_DP, &
            2.775854447439876E2_DP,  7.700015293522947E1_DP, &
            1.278272731962942E1_DP,  1.000000000000000_DP /
  DATA p3 /-2.996107077035422E-3_DP,-4.947309106232507E-2_DP, &
           -2.269565935396869E-1_DP,-2.786613086096478E-1_DP, &
           -2.231924597341847E-2_DP /
  DATA q3 / 1.062092305284679E-2_DP, 1.913089261078298E-1_DP, &
            1.051675107067932_DP,    1.987332018171353_DP,    &
            1.000000000000000_DP /

  DATA pim1 / 0.56418958354775629_DP /
  !        ( pim1= sqrt(1/pi) )
  ax = abs (x)
  if (ax > 26.0_DP) then
     !
     !  erfc(26.0)=10^(-296); erfc( 9.0)=10^(-37);
     !
     qe_erfc = 0.0_DP
  elseif (ax > 4.0_DP) then
     x2 = x**2
     xm2 = (1.0_DP / ax) **2
     qe_erfc = (1.0_DP / ax) * exp ( - x2) * (pim1 + xm2 * (p3 (1) &
          + xm2 * (p3 (2) + xm2 * (p3 (3) + xm2 * (p3 (4) + xm2 * p3 (5) &
          ) ) ) ) / (q3 (1) + xm2 * (q3 (2) + xm2 * (q3 (3) + xm2 * &
          (q3 (4) + xm2 * q3 (5) ) ) ) ) )
  elseif (ax > 0.47_DP) then
     x2 = x**2
     qe_erfc = exp ( - x2) * (p2 (1) + ax * (p2 (2) + ax * (p2 (3) &
          + ax * (p2 (4) + ax * (p2 (5) + ax * (p2 (6) + ax * (p2 (7) &
          + ax * p2 (8) ) ) ) ) ) ) ) / (q2 (1) + ax * (q2 (2) + ax * &
          (q2 (3) + ax * (q2 (4) + ax * (q2 (5) + ax * (q2 (6) + ax * &
          (q2 (7) + ax * q2 (8) ) ) ) ) ) ) )
  else
     qe_erfc = 1.0_DP - qe_erf (ax)
  endif
  !
  ! erf(-x)=-erf(x)  =>  erfc(-x) = 2-erfc(x)
  !
  if (x < 0.0_DP) qe_erfc = 2.0_DP - qe_erfc
  !
  return
end function qe_erfc
!
!---------------------------------------------------------------------
FUNCTION gauss_freq (x)
  !---------------------------------------------------------------------
  !
  !     gauss_freq(x) = (1+erf(x/sqrt(2)))/2 = erfc(-x/sqrt(2))/2
  !             - See comments in erf
  !
  IMPLICIT NONE
  INTEGER, PARAMETER :: DP=8
  REAL(DP), INTENT(IN) :: x
  REAL(DP)            :: gauss_freq
  REAL(DP), PARAMETER :: c =  0.7071067811865475_DP
  !        ( c= sqrt(1/2) )
  REAL(DP), EXTERNAL :: qe_erfc
  !
  gauss_freq = 0.5_DP * qe_erfc ( - x * c)
  !
  return
end function gauss_freq


      PROGRAM SFROID
      PARAMETER(NE=3,M=41,NB=1,NCI=NE,NCJ=NE-NB+1,NCK=M+1,NSI=NE,
     *    NSJ=2*NE+1,NYJ=NE,NYK=M)
      COMMON X(M),H,MM,N,C2,ANORM
      DIMENSION SCALV(NE),INDEXV(NE),Y(NE,M),C(NCI,NCJ,NCK),S(NSI,NSJ)
      ITMAX=100
      CONV=5.E-6
      SLOWC=1.
      H=1./(M-1)
      C2=0.
      WRITE(*,*)'ENTER M,N'
      READ(*,*)MM,N
      IF(MOD(N+MM,2).EQ.1)THEN
        INDEXV(1)=1
        INDEXV(2)=2
        INDEXV(3)=3
      ELSE
        INDEXV(1)=2
        INDEXV(2)=1
        INDEXV(3)=3
      ENDIF
      ANORM=1.
      IF(MM.NE.0)THEN
        Q1=N
        DO 11 I=1,MM
          ANORM=-.5*ANORM*(N+I)*(Q1/I)
          Q1=Q1-1.
11      CONTINUE
      ENDIF
      DO 12 K=1,M-1
        X(K)=(K-1)*H
        FAC1=1.-X(K)**2
        FAC2=FAC1**(-MM/2.)
        Y(1,K)=PLGNDR(N,MM,X(K))*FAC2
        DERIV=-((N-MM+1)*PLGNDR(N+1,MM,X(K))-(N+1)*
     *      X(K)*PLGNDR(N,MM,X(K)))/FAC1
        Y(2,K)=MM*X(K)*Y(1,K)/FAC1+DERIV*FAC2
        Y(3,K)=N*(N+1)-MM*(MM+1)
12    CONTINUE
      X(M)=1.
      Y(1,M)=ANORM
      Y(3,M)=N*(N+1)-MM*(MM+1)
      Y(2,M)=(Y(3,M)-C2)*Y(1,M)/(2.*(MM+1.))
      SCALV(1)=ABS(ANORM)
      SCALV(2)=MAX(ABS(ANORM),Y(2,M))
      SCALV(3)=MAX(1.,Y(3,M))
1     CONTINUE
      WRITE (*,*) 'ENTER C**2 OR 999 TO END'
      READ (*,*) C2
      IF(C2.EQ.999.)STOP
      CALL SOLVDE(ITMAX,CONV,SLOWC,SCALV,INDEXV,NE,NB,M,Y,NYJ,NYK,
     *    C,NCI,NCJ,NCK,S,NSI,NSJ)
      WRITE (*,*) ' M = ',MM,'  N = ',N,
     *    '  C**2 = ',C2,'  LAMBDA = ',Y(3,1)+MM*(MM+1)
      GO TO 1
      END


subroutine tend(im,km,Rd,Rv,Cpd,Cpv,g,p,u,t,q,z0,udot,tdot,qdot,z0dot,w)

! Compute tendencies of u, T, q

implicit none

! In
integer :: im,km,k
real :: Rd,Rv,Cpd,Cpv,g
real, dimension(im,km) :: p,u,t,q
real, dimension(im) :: z0

! Out
real, dimension(im) :: z0dot
real, dimension(im,km) :: udot,tdot,qdot
real, dimension(im,km+1) :: w

! Local
integer :: ks
real :: dx,dp,e,timescale
real, dimension(im,km) :: z,alpha,udot_pgt,tdot_adiab,ut_x,wt_p,uq_x,wq_p,Cpeff,theta

! Column width (m)
dx = 1000.e3
dp = p(1,2)-p(1,1)

! Compute some diagnostic fields
alpha = Rd*t/p*(1.-q+Rv*q/Rd)
call get_z(im,km,dp,g,p,alpha,z0,z)   ! geopotential height
call get_w(im,km,dx,dp,u,w)      ! omega

!---------------- 1) Momentum equation
udot_pgt(1,:) = -g*( z(2,:)-z(1,:) )/dx ! Pressure gradient term
udot_pgt(2,:) = udot_pgt(1,:)
udot = udot_pgt         ! Total tendency

! Sponge
!ks=sum(minloc(p/100.-200.),1)-1
!do k=1,ks
!e=float(k-1)/ks
!udot(:,k) = udot(:,k)  -0.5e-3 * (1.-e)**2*u(:,k)
!enddo

! Rayleigh friction
timescale=10.
!udot = udot -u/(timescale*86400.)

!----------------- 2) Heat equation
Cpeff = Cpd*(1.-q)+Cpv*q      ! moisture-adjusted heat capacity of air
call upwind(im,km,dx,dp,u,w,t,ut_x,wt_p)               ! Advection
tdot_adiab  = (w(:,1:km)+w(:,2:km+1))/2. * alpha/Cpeff    ! Adiabatic heating
tdot = -(ut_x + wt_p) + tdot_adiab

!----------------- 3) Moisture equation
call upwind(im,km,dx,dp,u,w,q,uq_x,wq_p)          ! Advection
qdot = -(uq_x + wq_p)

! surface height
z0dot = sum(alpha(:,km))/2./g*w(:,km+1)

end subroutine tend
!========================================================================
subroutine get_z(im,km,dp,g,p,alpha,z0,z)

! Compute geopotential height

implicit none
integer :: im,km,k
real :: g,dp,z0(im)
real, dimension(im,km) :: p,alpha,z

! Integrate upwards from surface
z(:,km) = z0 + alpha(:,km)/g*dp/2.
do k=km-1,1,-1
z(:,k) = z(:,k+1) + (alpha(:,k)+alpha(:,k+1))/2./g*dp
enddo

end subroutine get_z
!========================================================================
subroutine get_w(im,km,dx,dp,u,w)

! Solve continuity equation for w
! Set w=0 at top and integrate downwards
!
implicit none
integer :: im,km,k,ix
real :: dx,dp,u(im,km),w(im,km+1)

w=0. ! top boundary condition

! interior
do k=2,km+1
w(1,k) = w(1,k-1) - dp * u(1,k-1)/ dx
enddo

w(2,:) = -w(1,:)

end subroutine get_w
!========================================================================
subroutine upwind(im,km,dx,dp,u,w,f,uf_x,wf_p)

! compute upwind-differenced flux convergences
! of field f (defined on T grid)

implicit none
integer :: im,km,k,i,iz,ix
real :: dx,dp
real, dimension(im,km) :: u,f,uf,uf_x,wf_p
real, dimension(im,km+1) :: w,wf

uf=0.
wf=0.

! Horizontal
do k=1,km
ix=( 1-nint(sign(1.,u(1,k))) )/2 ! u>=0 => ix=0; u<0 => ix=1
uf(1,k)=u(1,k)*f(1+ix,k)
enddo
uf_x(1,:) =  uf(1,:) / dx
uf_x(2,:) = -uf(1,:) / dx

! Vertical
do i=1,im
 do k=2,km
 iz=( 1-nint(sign(1.,w(i,k))) )/2 - 1 ! w>=0 => iz=-1; w<0 => iz=0
 wf(i,k)=w(i,k)*f(i,k+iz)
 enddo
 ! -- bottom
 wf(i,km+1) = w(i,km+1)*f(i,km)
enddo
wf_p(:,:) = ( wf(:,2:km+1)-wf(:,1:km) ) / dp

end subroutine upwind


!>@brief The module 'epslon_stochy_mod' contains the subroute epslon_stochy
      module epslon_stochy_mod

      implicit none

      contains

!>@brief The subroutine 'epslon_stochy' calculate coeffients for use in spectral space
!>@details This code is taken from the legacy spectral GFS
      subroutine epslon_stochy(epse,epso,epsedn,epsodn,
     &                  ls_node)
cc
      use spectral_layout_mod
      use kinddef
      implicit none
cc
      real(kind_dbl_prec)   epse(len_trie_ls)
      real(kind_dbl_prec)   epso(len_trio_ls)
cc
      real(kind_dbl_prec) epsedn(len_trie_ls)
      real(kind_dbl_prec) epsodn(len_trio_ls)
cc
      integer                  ls_node(ls_dim,3)
cc
!cmr  ls_node(1,1) ... ls_node(ls_max_node,1) : values of L
!cmr  ls_node(1,2) ... ls_node(ls_max_node,2) : values of jbasev
!cmr  ls_node(1,3) ... ls_node(ls_max_node,3) : values of jbasod
cc
      integer                  l,locl,n
cc
      integer                  indev
      integer                  indod
cc
      real(kind_dbl_prec) f1,f2,rn,val
cc
      real(kind_dbl_prec) cons0     !constant
cc
      integer                  indlsev,jbasev
      integer                  indlsod,jbasod
cc
      include 'function2'
cc
cc
      cons0=0.0d0     !constant
cc
cc
cc......................................................................
cc
cc
      do locl=1,ls_max_node
              l=ls_node(locl,1)
         jbasev=ls_node(locl,2)
         indev=indlsev(l,l)
         epse  (indev)=cons0     !constant
         epsedn(indev)=cons0     !constant
          indev=indev+1
cc

         do n=l+2,jcap+1,2
            rn=n
            f1=n*n-l*l
            f2=4*n*n-1
            val=sqrt(f1/f2)
            epse  (indev)=val
            epsedn(indev)=val/rn
             indev=indev+1
         enddo
cc
      enddo
cc
cc
cc......................................................................
cc
cc
      do locl=1,ls_max_node
              l=ls_node(locl,1)
         jbasod=ls_node(locl,3)
         indod=indlsod(l+1,l)
cc
         do n=l+1,jcap+1,2
            rn=n
            f1=n*n-l*l
            f2=4*n*n-1
            val=sqrt(f1/f2)
            epso  (indod)=val
            epsodn(indod)=val/rn
             indod=indod+1
         enddo
cc
      enddo
cc
      return
      end

      end module epslon_stochy_mod


!-------------------------------------------------------------------------------
! File: MeltingRD.f90
! Written by: ML, June 2018
! Modified by: -
!-------------------------------------------------------------------------------
FUNCTION MeltingRD(Model, n, f) RESULT(g)
USE DefUtils
TYPE(Model_t) :: Model
INTEGER :: n
REAL(KIND=dp) :: f, g, x

x = Model % Nodes % x(n)
!write(*,*) 'This is 50.0 *f', 5.0*f
g = 0.3 + 1.0*EXP(-((x-15.0*f)**2)/300000.0)
END FUNCTION MeltingRD


subroutine getee

! generates a matrix with ndim columns and base**ndim rows
! which can be used to transform ndim nested loops (all indices
! running 0 to 1) into a single loop
!
! e.g. for base=2
! do i=0,1
!	do j=0,1
!		do k=0,1
!			write(*,*)i,j,k
!		enddo
!	enddo
! enddo
!
! is equivalent to
! do i=1,2**3
!	write(*,*)ee(1,i),ee(2,i),ee(3,i)
! enddo
!
!
! The matrix eeindi is a copy of ee with its columns
! rearranged according to the interpolation order (indi)
! (they are identical for the default indi values, i.e.
!  for indi = ndim, ndim-1, ndim-2 ...)
!
! The array imap tracks the row in ee which corresponds to
! a given row in eeindi. It allows to sort the wrk array in
! the interpolation routines to match the desired interpolation
! order.

use share, only: ndim,inter,indi,ee,imap
implicit none

integer			:: i,j,k,nr,c1,c2,base
integer			:: istat	!allocate status var
integer, allocatable 	:: eeindi(:,:)  !copy of ee with columns
					!swapped according to indi
integer 		:: powers(ndim) !(2**0, 2**1, 2**2...)

select case (inter)
	case (1,2,3)
		base=inter+1 !lin,quad., cubic
	case (4)
		base=4       !cubic splines
	case default
		base=2
end select

allocate (ee(ndim,base**ndim),stat=istat)
call checkstat(istat,'ee')
allocate (eeindi(ndim,base**ndim),stat=istat)
call checkstat(istat,'eeindi')
allocate (imap(base**ndim),stat=istat)
call checkstat(istat,'imap')

do i=1,ndim
	nr=base**(i-1)
	c1=1
	do j=1,nr
		c2=1
		do k=1,base**ndim/nr
			ee(i,c1)=(c2-1)/base**(ndim-i)
			!write(*,*)i,ee(i,c1)
			c1=c1+1
			c2=c2+1
		enddo
	enddo
enddo

!if (indi(1) /= 0) then
	do i=1,ndim
		powers(i)=base**(ndim-i)
	enddo

	!copy ee to eeindi and swap columns to match interpolation order
	eeindi=ee
	do i=1,ndim
		eeindi(i,1:base**ndim)=ee(ndim-indi(i)+1,1:base**ndim)
	enddo
	do i=1,base**ndim
		imap(i)=dot_product(eeindi(1:ndim,i),powers)+1
	enddo
!endif

!do i=1,base**ndim
!	write(*,*)'ee=',ee(1:ndim,i)
!enddo
!do i=1,base**ndim
!	write(*,*)'eeindi=',eeindi(1:ndim,i)
!enddo
!
!do i=1,base**ndim
!	write(*,*)'imap=',imap(i)
!enddo

deallocate(eeindi)

end subroutine getee


      program testprogram
      integer n
      parameter (lda = 5, ldb = 5, n = 5)
c
      integer j
      double precision alpha, beta
      double precision w(n), x(n), y(n), z(n), xorig(n), q(n), s(n)
      double precision u1(n), u2(n), v1(n), v2(n), worig(n), yorig(n)
      double precision A(lda, n), B(lda, n)
      data alpha/22/, beta/7/
      data worig/1, 2, 3, 4, 5/, xorig/5, 10, 15, 20, 25/
      data z/2, 4, 6, 8, 10/, yorig/0, 3, 5, 7, 9/
      data u1/1, 2, 3, 4, 5/, u2/5, 10, 15, 20, 25/, v1/0, 3, 5, 7, 9/
      data v2/2, 4, 6, 8, 10/
      data A/1,2,3,4,5,2,3,4,5,6,0,1,0,1,1,1,1,1,1,1,2,1,2,1,2/
      data B/10,2,3,4,5,2,3,4,5,6,0,1,0,1,1,1,1,1,1,1,2,1,2,1,20/
c
c    x = w + y + z
c
      do j = 1,n
        x(j) = xorig(j)
        y(j) = yorig(j)
        w(j) = worig(j)
      enddo
      call vadd(n, x, w, y, z)
      print*,'vadd result x',(x(j),j=1,n)
      print*,'vadd result should be (3, 9, 14, 19, 24)'
      print*,' '
      do j = 1,n
        x(j) = xorig(j)
      enddo
c
c     w = alpha * x + beta * y
c
      do j = 1,n
        x(j) = xorig(j)
        w(j) = worig(j)
        y(j) = yorig(j)
      enddo
      call waxpby(n, w, alpha, x, beta, y)
      print*,'waxpby result w',(w(j),j=1,n)
      print*,'vadd result should be (110   241   365   489   613)'
      print*,' '
      do j = 1,n
        w(j) = worig(j)
      enddo
c
c     q = A * x
c     s = A' * y
c
      call bicgkernel(lda, n, A, x, y, s, q)
      print*,'bicgkernel result q',(q(j),j=1,n)
      print*,'bicgkernel result s',(s(j),j=1,n)
      print*,'bicgkernel result should be (95 100 125 130 170)'
      print*,'bicgkernel result should be (94 118 19 24 38)'
      print*,' '
c
c     y = alpha * (A * x) + beta * (B * x)
c
      do j = 1,n
        y(j) = yorig(j)
      enddo
      call gesummv(alpha, beta, n, lda, a, ldb, b, x, y)
      print*,'gesummv 1st y should be (980 700 875 910 1190)'
      print*,'gesummv 2nd y should be (3070 2900 3625 3770 8080)'
      print*,' '
      do j = 1,n
        y(j) = yorig(j)
      enddo
c
c
c     B = A + u1 * v1' + u2 * v2'
c     x = beta * (B' * y) + z
c     w = alpha * (B * x)
c
      call gemver(alpha,beta,lda,n,A,ldb,B,u1,v1,u2,v2,w,x,v1,z)
      print*,'gemver result x',(x(j),j=1,n)
      print*,'gemver result w',(w(j),j=1,n)
      print*,'x should be 7240.  15964.  23169.  31102.  39098.'
      print*,'w should be 113685638. 224265074. 335545386. '
      print*, '446124822. 557914852.'
      print*,' '
c
c  y = A' * (A * x)
c
      do j = 1,n
        x(j) = xorig(j)
      enddo
      call waxpby(n, w, alpha, x, beta, y)
      call atax(lda, n, a, x, y, z)
      print*,'atax result y',(y(j),j=1,n)
      print*,'atax result y should be  2040.  2660.  400.  620.  1010.'
c
      end


module mod

contains

subroutine sum(x, res)
  implicit none
  real, intent(in) :: x(:)
  real, intent(out) :: res

  integer :: i

  !print *, "sum: size(x) = ", size(x)

  res = 0.0

  do i = 1, size(x)
    res = res + x(i)
  enddo

end subroutine sum

function fsum(x) result (res)
  implicit none
  real, intent(in) :: x(:)
  real :: res

  integer :: i

  !print *, "fsum: size(x) = ", size(x)

  res = 0.0

  do i = 1, size(x)
    res = res + x(i)
  enddo

end function fsum

end module mod


!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!                                                                     !
!                                Lucifer                              !
!                                                                     !
!                         Jason Nguyen (XXXXXXX)                      !
!                                                                     !
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
      program luchex
      use hex
      implicit none

!   Temp counter used to iterate through arrays
      integer :: i

!   kb is the raw input from the user, representing the 32 bytes
!   kb is then expanded into key, from an array of bytes to binary.
!
!   Now, we need to read key as an array of 8 x 16, not just a line
!   of 128 binary integers. So we later reshape key (1 x 128) into
!   k (8 x 16) by means of reshape()
      integer, dimension (0:31) :: kb
      integer, dimension (0:127) :: key
      integer, dimension (0:7, 0:15) :: k

!   The same story applies here. We read 32 bytes into the message
!   plaintext, and then we use expand() to turn mb, an array of 32
!   bytes, into a binary representation of 128 binary digits. The
!   only difference here is that instead of the 1 x 128 array being
!   mapped onto an 8 x 16 array, we are mapping it to 8 x 8 x 2
      integer, dimension (0:31) :: mb = 0
      integer, dimension (0:127) :: message = 0
      integer, dimension (0:7, 0:7, 0:1) :: m = 0

!   Here is the string used for the hex component
      character (len = 11) :: w

!   I think this variable is useless, but I will create it anyway
!   so I don't potentially use marks. I zero-initialize my mb array
!   so I should be able to refer to it as length 32 without issue
      integer :: hexlength = 0

!   Error checking
      integer :: readerror = 1

!   Prompt the user for the key first
      print *, '~~~~~~~~~~User Input~~~~~~~~~~~'
      do
         print *, ' '
         print *, 'Enter your key (0-9, A-F and a-f only):'
         read(*, '(32z1.1)', iostat = readerror) (kb(i),i=0,31)

         if (readerror == 0) then
            exit
         end if

         print *, 'Invalid input. Please try again.'
      end do

!   Prompt the user for the plaintext word
      print *, ' '
      do while (len_trim(w) < 1 .or. len_trim(w) > 10)
         print *, 'Enter your word (alphabetic only):'
         call readWord(w)
         if (len_trim(w) < 1 .or. len_trim(w) > 10) then
            print *, 'Error: word must be between 1 and 10 chars.'
            print *, ' '
         end if
      end do

!   Convert the user-inputted string to hex array
!   I am *re-using* the mb variable as we no longer use it for
!   hard 32-char input like in luc.f95
      call word2hex(w, mb, hexlength)

!   Print the word in hex
      print *, ' '
      print *, 'Hexadecimal word'
      call printhex(mb, hexlength)
      print *, ' '

!   Expand both the message (mb) and key (kb) as explained above
      call expand(message, mb, 32)
      call expand(key, kb, 32)

!   Reshape key and message from (1 x 128) to their respective
!   mappings. (8 x 16) for key, (8 x 8 x 2) for message.
      k = reshape(key, [8, 16])
      m = reshape(message, [8, 8, 2])

!   First we encipher with the lucifer function. This is shown
!   by the first argument being 0.
      call lucifer(0, k, m)

!   Begin section 'Encrypted message'
      print *, ' '
      print *, '~~~~~~~Encrypted message~~~~~~~'
      print *, ' '

!   Print ciphertext
      print *, 'Ciphertext:'
      call compress(m, mb, 32)
      print '(32z1.1)', (mb(i), i = 0, 31)

!   Next, we decipher with the lucifer function. This is shown
!   by the first argument being 1.
      call lucifer(1, k, m)

!   Finally, we compress the resultant message and key
      call compress(message, mb, 32)
      call compress(key, kb, 32)

!   Begin section 'Decrypted message'
      print *, ' '
      print *, '~~~~~~~Decrypted message~~~~~~~'
      print *, ' '

!   Then we print them both again for debugging purposes . . .
      print *, 'Key:'
      print '(32z1.1)', (kb(i), i = 0, 31)

!   . . . so if the program ran successfully, the key and plain
!   should be identical to the key and plain at the beginning
      print *, ' '
      print *, 'Plaintext:'
      print '(32z1.1)', (mb(i), i = 0, 31)
      print *, ' '
      print *, '~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~'

      end program luchex

!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

      subroutine lucifer(d,k,m)
!   This algorithm uses Feistel rounds. As in, it encrypted the data
!   by performing an encipherment step on it several times. This
!   encipherment step involved taking the key for that step, and half
!   of the block in order to produce a result to be applied to the XOR
!   of the remnant half of the block.
!
!   What made this solution particularly elegant was that the two
!   halves of the block would be switched after each step, allowing
!   each subsequent operation of the same type to evenly operate on all
!   parts of the respective block.
      implicit none

!   Function arguments
      integer, intent(in) :: d ! mode; 0 = cipher, 1 = decipher
      integer, dimension (0:7, 0:15), intent(in) :: k ! key
      integer, dimension (0:7, 0:7, 0:1), intent(inout) :: m ! message

!   Diffusion pattern
      integer, dimension (0:7) :: o = [7, 6, 2, 1, 5, 0, 3, 4]

!   Inverse of fixed permutation
      integer, dimension (0:7) :: tr, pr = [2, 5, 4, 0, 3, 1, 7, 6]

!   S-box permutations, S_0
      integer, dimension (0:15) :: s0 = [12, 15, 7, 10, 14, 13, 11, &
              0, 2, 6, 3, 1, 9, 4, 5, 8]

!   S-box permutations, S_1
      integer, dimension (0:15) :: s1 = [7, 2, 14, 9, 3, 11, 0, 4, &
              12, 13, 1, 10, 6, 15, 8, 5]

!   Other variables
      integer :: h0, h1, h, ii, jj, kc, kk, ks, l, temp, v
      integer, dimension (0:7,0:7) :: swap_temp

!   Unlike the original implementation that involved physically moving
!   information, this implementation saves the effort by using pointers
!   to indicate which part of the array(s) the algorithm works on.

!   The halves of the message byte selected are used as input to the
!   S_0 and S_1 permutations in order to create 4 v-bits each.

!   When k(jj, ks) = 0, the low-order  4 bits are used with s0
!                       the high-order 4 bits are used with s1

!   When k(jj, ks) = 1, the low-order  4 bits are used with s1
!                       the high-order 4 bits are used with s0

!      +-------------------+-----+-----+
!      |                   |  0  |  1  |
!      +-------------------+-----+-----+
!      | Low-Order 4 Bits  | s_0 | s_1 |
!      +-------------------+-----+-----+
!      | High-Order 4 Bits | s_1 | s_0 |
!      +-------------------+-----+-----+

!   Like stated earlier, we only point to the halves of the block:

      h0 = 0 !   lower half of the block
      h1 = 1 !   upper half of the block

      if (d == 1) then
         kc = 8
      else
         kc = 0
      end if

!   Sorkin's variant of Lucifer uses 16 Feistel rounds
      do ii = 1, 16

!        c-i-d (confusion, interruption, diffusion) cycle
         if (d == 1) then
            kc = mod(kc + 1, 16)
         end if

!        ks is the index of the 'transform control byte'
         ks = kc

         do jj = 0, 7
            l = 0
            h = 0

!           Here we construct the integer values of the hexdigits of
!           one byte of the message. We could have used compress(),
!           but this method is somewhat faster.

!           h and l were originally references to c(0) and c(1) as
!           per the equivalence, but I removed those out of clarity.
            do kk = 0, 3
               l = l * 2 + m(7 - kk, jj, h1)
            end do

            do kk = 4, 7
               h = h * 2 + m(7 - kk, jj, h1)
            end do

!           The document in verbatim referred to this as "controlled
!           interchange and s-box permutation, whatever that means.
            v = (s0(l) + 16*s1(h)) * (1 - k(jj, ks)) + (s0(h) + 16 * &
                    s1(l)) * k(jj, ks)

!           Here we convert v back into bit array format. We could have
!           used expand(), but this is faster.
            do kk = 0, 7
               tr(kk) = mod(v, 2)
               v = v / 2
            end do

!           Here we do key-interruption and diffusion, combined. The
!           "k + tr" term is the permuted key interruption.
!
!           mod(o(kk) + jj, 8) is the diffusion row for column kk.
!           row = byte and column = bit within byte
            do kk= 0, 7
               m(kk, mod(o(kk) + jj, 8), h0) = mod(k(pr(kk), kc) + &
                       tr(pr(kk)) + m(kk, mod(o(kk) + jj, 8), h0), 2)
            end do

            if (jj < 7 .or. d == 1) then
               kc = mod(kc + 1, 16)
            end if

         end do

!        As we established earlier, h0 and h1 are merely pointers to
!        the halves of the blocks we intend to work on. As we have
!        reached the end of the Feistel round, we have to swap h0 and
!        h1. We do so by using temp as a temporary buffer variable.
         temp = h0
         h0 = h1
         h1 = temp

      end do ! end of Feistel round

!     Here we physically swap the upper and lower halves of the message
!     after the last of the sixteen Feistel rounds completes.
      do jj = 0,7
         do kk = 0,7
            swap_temp(kk, jj) = m(kk, jj, 0)
            m(kk, jj, 0) = m(kk, jj, 1)
            m(kk, jj, 1) = swap_temp(kk, jj)
         end do
      end do

      return
      end subroutine lucifer

!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

      subroutine expand(a, b, l)
      implicit none

      integer, dimension(0:*), intent(out) :: a ! array in bit format
      integer, dimension(0:*), intent(in) :: b ! array in byte format
      integer, intent(in) :: l ! length of the array, b, in hexdigits

      integer :: i, j, v ! temporary variables

      do i = 0, l - 1
         v = b(i)
         do j = 0, 3
            a((3 - j) + i * 4) = mod(v, 2)
            v = v / 2
         end do
      end do

      return
      end

!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

      subroutine compress(a,b,l)
      implicit none

      integer, dimension(0:*), intent(in) :: a   ! array in bit format
      integer, dimension(0:*), intent(out) :: b ! array in byte format
      integer, intent(in) :: l  ! length of the array, b, in hexdigits

      integer :: i, j, v

      do i=0,l-1,1
         v=0
         do j=0,3,1
            v = v * 2 + mod(a(j + i * 4), 2)
         end do
         b(i)=v
      end do

      return
      end

!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!


c Begin commentary

        real function hypo(x, y)
        real x, y
        hypo = (x * x + y * y) ** (1.0/2.0)
        end

        program abc
        integer a
        real b, c
        print *, "This program calculates a ** b + c, type a, b, c:"
        read(*, *) a, b, c
        print *, a ** b + c

        if (a.ge.1) then
            write (*,*) """a"" greater than 1, it's", a, "you are crazy"
        elseif (b.le.1) then
            write (*,*) "Let's make ""b"" greater by 3", b+3
        else
            write (*,*) "Calculate hypo for ""b"" and ""c""", hypo(b, c)
        endif

        do i = 0, 3*(c-1), 3
            write(*, *) "Print this message exactly c times", i
        enddo

        end

c Some end commentary


      real function ppvalu (break, coef, l, k, x, jderiv )
c  from  * a practical guide to splines *  by c. de boor
calls  interv
calculates value at  x	of  jderiv-th derivative of pp fct from pp-repr
c
c******  i n p u t  ******
c  break, coef, l, k.....forms the pp-representation of the function  f
c	 to be evaluated. specifically, the j-th derivative of	f  is
c	 given by
c
c     (d**j)f(x) = coef(j+1,i) + h*(coef(j+2,i) + h*( ... (coef(k-1,i) +
c			      + h*coef(k,i)/(k-j-1))/(k-j-2) ... )/2)/1
c
c	 with  h = x - break(i),  and
c
c	i  =  max( 1 , max( j ,  break(j) .le. x , 1 .le. j .le. l ) ).
c
c  x.....the point at which to evaluate.
c  jderiv.....integer giving the order of the derivative to be evaluat-
c	 ed.  a s s u m e d  to be zero or positive.
c
c******  o u t p u t  ******
c  ppvalu.....the value of the (jderiv)-th derivative of  f  at  x.
c
c******  m e t h o d  ******
c     the interval index  i , appropriate for  x , is found through a
c  call to  interv . the formula above for the	jderiv-th derivative
c  of  f  is then evaluated (by nested multiplication).
c
      integer jderiv,k,l,   i,m,ndummy
      real break(l),coef(k,l),x,   fmmjdr,h
      ppvalu = 0.
      fmmjdr = k - jderiv
c	       derivatives of order  k	or higher are identically zero.
      if (fmmjdr .le. 0.)		go to 99
c
c	       find index  i  of largest breakpoint to the left of  x .
      call interv ( break, l, x, i, ndummy )
c
c      evaluate  jderiv-th derivative of  i-th polynomial piece at  x .
      h = x - break(i)
      m = k
    9	 ppvalu = (ppvalu/fmmjdr)*h + coef(m,i)
	 m = m - 1
	 fmmjdr = fmmjdr - 1.
	 if (fmmjdr .gt. 0.)		go to 9
   99					return
      end


program problem59
  implicit none
  integer, parameter      :: n=1201
  integer*1, dimension(n) :: numbers
  integer                 :: frequency(0:255)
  integer*1, dimension(3) :: key
  integer                 :: i,j,temp,count

  open(33,file='./data/problem59.dat')
  read(33,*) numbers
  close(33)

  do j=1,3
     count=0

     do i=0,255
        frequency(i)=0
     end do

     do i=j,n,3
        temp=frequency(numbers(i))+1
        frequency(numbers(i))=temp
        if (temp>count) then
           count=temp
           key(j)=numbers(i)
        end if
     end do
     key(j)=xor(key(j),ichar(' '))
  end do

  count=0
  do i=1,n
     temp=xor(numbers(i),key(mod(i-1,3)+1))
     count=count+temp
  end do

  print *, count

end program problem59


submodule (stats) stats_sm

contains

    module real(8) function get_bin_size(this)
        class(histogram),  intent(in) :: this
        get_bin_size = (this%bin_max-this%bin_min)/this%nbins
    end function get_bin_size

    ! Create a histogram given the number of bins
    module subroutine create_hist1(this, col, mask, nbins)
        class(histogram), intent(out) :: this
        real(8), intent(in)           :: col(:)
        logical, intent(in), optional :: mask(:)
        integer, intent(in), optional :: nbins
        logical, allocatable :: hist_mask(:)      ! Mask selecting those entries in col that are in the current interval
        real(8)              :: min_col, max_col, bin_size
        real(8), allocatable :: col_data(:)
        integer              :: i

        if (present(nbins)) then
            this%nbins = nbins
        end if

        ! Create local contiguous copy of the data being processed
        if (present(mask)) then
            col_data = pack(col,mask)
        else
            col_data = col
        end if

        ! Initialize the frequency
        allocate(this%frequency(0:this%nbins-1))

        ! Calculate the range of data in 'col' for each bin
        max_col = maxval(col_data)
        min_col = minval(col_data)
        if (max_col == min_col) then
            max_col = max_col + 0.5d0
            min_col = min_col - 0.5d0
        end if
        bin_size = (max_col-min_col)/this%nbins
        do i=0, this%nbins-1
            hist_mask  = (min_col + i*bin_size) < col_data .and. col_data <= (min_col + (i+1)*bin_size)
            this%frequency(i) = count(hist_mask)/bin_size
        end do
        this%bin_max = max_col
        this%bin_min = min_col
    end subroutine create_hist1

    ! Create a histogram given the bin size and range
    module subroutine create_hist2(this, col, bin_size, bin_range, mask)
        class(histogram), intent(out) :: this
        real(8), intent(in)           :: col(:)
        real(8), intent(in)           :: bin_size
        real(8), intent(in)           :: bin_range(2)
        logical, intent(in), optional :: mask(:)
        real(8), allocatable :: col_data(:)
        logical, allocatable :: hist_mask(:)      ! Mask selecting those entries in col that are in the current interval
        integer              :: i

        ! Create local contiguous copy of the data being processed
        if (present(mask)) then
            col_data = pack(col,mask)
        else
            col_data = col
        end if

        ! Initialize the frequency
        this%bin_max = bin_range(2)
        this%bin_min = bin_range(1)
        this%nbins = nint((this%bin_max-this%bin_min)/bin_size)
        allocate(this%frequency(0:this%nbins-1))

        ! Calculate the range of data in 'col' for each bin
        do i=0, this%nbins-1
            hist_mask  = (this%bin_min + i*bin_size) < col_data .and. &
                          col_data <= (this%bin_min + (i+1)*bin_size)
            this%frequency(i) = count(hist_mask)/bin_size
        end do
    end subroutine create_hist2

    ! Create a histogram given the bin size and range
    module subroutine create_hist3(this, col, bin_size, bin_range, mask)
        class(histogram), intent(out) :: this
        real(8), intent(in)           :: col(:)
        integer, intent(in)           :: bin_size
        integer, intent(in)           :: bin_range(2)
        logical, intent(in), optional :: mask(:)
        real(8)          :: r_bin_range(2)

        ! Check the range
        if (mod(bin_range(2)-bin_range(1),bin_size) /= 0) then
            stop '***Error[create_hist3]: inconsistent bin definition'
        end if

        r_bin_range = real(bin_range,8)
        call this%create(col, real(bin_size,8), r_bin_range, mask)
        this%int_bin_size = .true.
    end subroutine create_hist3

    ! Write histogram
    module subroutine write_histogram(this, u)
        class(histogram), intent(in) :: this
        integer, intent(in)          :: u
        integer :: i
        real(8) :: bin_size

        bin_size = (this%bin_max - this%bin_min)/this%nbins
        associate (bin_start=>this%bin_min, freq=>this%frequency)
            if (this%int_bin_size) then
                do i=0, this%nbins-1
                    write(u,'(2(a,i0),a,f0.2,a,f0.2)') '(',nint(bin_start+i*bin_size), &
                    '->',nint(bin_start+(i+1)*bin_size),'],',bin_start+i*bin_size+bin_size/2,',',freq(i)
                end do
            else
                do i=0, this%nbins-1
                    write(u,'(2(a,es9.2),a,es9.2,a,es9.2)') '(',bin_start+i*bin_size,'->', &
                    bin_start+(i+1)*bin_size,'],',bin_start+i*bin_size+bin_size/2,',',freq(i)
                end do
            end if
        end associate
    end subroutine write_histogram

    ! Calculate mean and standard deviation
    module subroutine calculate_stats(col, mean, mask, standard_deviation, skewness)
        real(8), intent(in)            :: col(:)
        real(8), intent(out)           :: mean
        logical, intent(in), optional  :: mask(:)
        real(8), intent(out), optional :: standard_deviation
        real(8), intent(out), optional :: skewness
        real(8) :: mv(3), ssdev
        integer :: i, n

        mv = 0
        if (present(mask)) then
            n = count(mask)
            if (n == 0) return

            !$omp parallel do shared(col, mask) reduction(+:mv)
            do i=1,size(col)
                if (mask(i)) then
                    mv(1) = mv(1) + col(i)
                    if (present(standard_deviation) .or. present(skewness)) then
                        mv(2) = mv(2) + col(i)**2
                        if (present(skewness)) then
                            mv(3)= mv(3)+ col(i)**3
                        end if
                    end if
                end if
            end do
            !$omp end parallel do

        else
            n = size(col)
            !$omp parallel do shared(col) reduction(+:mv)
            do i=1,n
                mv(1) = mv(1) + col(i)
                if (present(standard_deviation) .or. present(skewness)) then
                    mv(2) = mv(2) + col(i)**2
                    if (present(skewness)) then
                        mv(3)= mv(3)+ col(i)**3
                    end if
                end if
            end do
            !$omp end parallel do

        end if

        ! Mean and standard deviation
        mv = mv/n
        mean = mv(1)
        if (present(standard_deviation) .or. present(skewness)) then
            ssdev = sqrt(abs(mv(2) - mv(1)**2))  ! Rounding could make it < 0 so use abs to be safe
            if (present(skewness)) then
                skewness = (mv(3) - mean*(3*ssdev**2 + mean**2))/ssdev**3
            end if
            if (present(standard_deviation)) then
                standard_deviation = ssdev
            end if
        end if

    end subroutine calculate_stats

    module function random() result(ran)
!=======================================================================
!
!    This is an adapted version of subroutine RANECU written by
!    F. James (Comput. Phys. Commun. (1990) 60, 329-344), which
!    has been modified to give a single random number at each call.
!    The seeds iseed1 and iseed2 must be initialized in the main
!    program and transferred through the named common block /REED/.
!
!=======================================================================

        real(8) :: ran

        ! ---   Declaracion de variables   ---
        integer :: i1, i2, iz

        ! Saved data. Note that inline initialization implies 'save'
        integer :: iseed1 = 860934
        integer :: iseed2 = 542039

        ! ---   Parametros   ---
        real(8), parameter :: uscale=1.0d0/2.0d0**31

!     ---   Calculo del numero aleatorio   ---

        i1 = iseed1/53668
        iseed1 = 40014*(iseed1-i1*53668)-i1*12211

        if (iseed1 < 0) then
            iseed2 = iseed1+2147483563
        end if

        i2 = iseed2/52774
        iseed2 = 40692*(iseed2-i2*52774)-i2*3791

        if (iseed2 < 0) then
            iseed2 = iseed2+2147483399
        end if

        iz = iseed1-iseed2

        if (iz < 1) then
            iz = iz+2147483562
        end if

        ran = iz*uscale
        return

    end function random

!***********************************************************************

    module function gasdev()
!=======================================================================
!
!     Returns a normally distributed deviate with zero mean and unit
!     variance.
!
!=======================================================================

        real(8) :: gasdev

        ! ---   Variable declaration   ---
        real(8)       :: fac, r
        real(8)       :: v1, v2

        ! ---   Use static memory for persistence   ---
        real(8), save :: gset = 0
        logical, save :: have_extra_deviate = .false.

!       ---   We don't have an extra deviate handy, so ...   ---
        if (.not. have_extra_deviate) then

            uniform_square: do

                ! ---   pick two uniform numbers in the square (-1,+1)

                v1 = 2*random()-1
                v2 = 2*random()-1

                ! ---   See if they are in the unit circle   ---

                r = v1*v1+v2*v2

                ! ---   and if they are not, try again   ---

                if (r < 1 .and. r > 0) exit uniform_square

            end do uniform_square

!           ---  Make Box-Muller polar transformation to get 2 normal deviates   ---

            fac = sqrt(-2*log(r)/r)

!           ---   return one and save the other for the next time   ---

            gset = v1*fac
            gasdev = v2*fac
            have_extra_deviate = .true.

        else

!           ---   We have an extra deviate so return it and unset flag   ---

            gasdev = gset
            have_extra_deviate = .false.

        end if

        return

    end function gasdev

!***********************************************************************

end submodule stats_sm


C     PROGRAM FOR FINDING THE AVERAGE OF A
C    X    SET OF NUMBERS
      DIMENSION A(999)
    1 FORMAT (I3/(F6.3))
      READ 1 N, (A(J), J = 1, N)
      SUM = 0.0
      DO 6 I = 1, N
      IF (A(I)) 5, 2, 2
    2 SUM = SUM + A(I)
    3 FORMAT (F6.3)
      PRINT 3 A(I)
      CONTINUE
    4 FORMAT (21HNEGATIVE NUMBER FOUND)
    5 PRINT 4
    6 CONTINUE
    7 FORMAT (9HAVERAGE: F6.3)
      PRINT 7 SUM / N


FUNCTION ff( x ,y )
!-----------------------------------------------------------------------
!
!    File:         ff
!    Module:       ff
!    Type:         Function
!    Author:       S. W. Bruenn, Dept of Physics, FAU,
!                  Boca Raton, FL 33431-0991
!
!    Date:         7/28/02
!
!    Purpose:
!      To multiply two fermi functions
!
!----------------------------------------------------------------------c

USE kind_module, ONLY: double
USE numerical_module, ONLY: one

IMPLICIT NONE

!-----------------------------------------------------------------------
!        Input variables.
!-----------------------------------------------------------------------

REAL(KIND=double), INTENT(IN)  :: x               ! function argument
REAL(KIND=double), INTENT(IN)  :: y               ! function argument

!-----------------------------------------------------------------------
!        Output variables
!-----------------------------------------------------------------------

REAL(KIND=double)              :: ff              ! declare function

!-----------------------------------------------------------------------
!        Local variables
!-----------------------------------------------------------------------

REAL(KIND=double)              :: fexp            ! exponential

EXTERNAL fexp

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

ff              = one/( ( one + fexp(x) ) * ( one + fexp(y) ) )

END FUNCTION ff


      subroutine qlbox15(Y,musq,Ires)
      implicit none
C 15:
C I_4^({D=4-2\e})(m_1^2,\pd^2,\pt^2,m_4^2;s_{12},s_{23};0,m_2^2,0,m_4^2)

c       [                                  s12                         ]
c       [   0             0              - ---              0          ]
c       [                                   2                          ]
c       [                                                              ]
c       [                             m2sq - p2sq  - s23 + m4sq + m2sq ]
c       [   0           m2sq          -----------  ------------------- ]
c       [                                  2                2          ]
c y15 = [                                                              ]
c       [   s12      m2sq - p2sq                       m4sq - p3sq     ]
c       [ - ---      ----------           0           -----------     ]
c       [    2            2                                 2          ]
c       [                                                              ]
c       [        - s23 + m4sq + m2sq  m4sq - p3sq                      ]
c       [   0    -------------------  -----------         m4sq         ]
c       [                 2                2                           ]

C----Implementation of Bennakker-Denner Eq.2.11
C----\bibitem{Beenakker:1988jr}
C----W.~Beenakker and A.~Denner,
C----%``INFRARED DIVERGENT SCALAR BOX INTEGRALS WITH APPLICATIONS IN THE
C----%ELECTROWEAK STANDARD MODEL,''
C----Nucl.\ Phys.\  B {\bf 338}, 349 (1990).

      include 'qlconstants.f'
      integer iep
      logical qlzero,qlnonzero
      double precision Y(4,4),m2sq,m4sq,musq,m2,m4,ta,
     . si,m2sqbar,m4sqbar,ieps,iepyy,iepyi,yy,yi,imxs,rexs
      double complex xs,cxs(3),qlcLi2omx2,fac,Ires(-2:0),qllnrat,xlog,
     . cln,cyy,cyi

      m2sq=Y(2,2)
      m4sq=Y(4,4)
      m2sqbar=2d0*Y(2,3)
      m4sqbar=2d0*Y(3,4)
C-----Assign s and t so as to agree with notation of BD
      si=2d0*Y(2,4)-Y(2,2)-Y(4,4)
      ta=2d0*Y(1,3)
      m2=sqrt(m2sq)
      m4=sqrt(m4sq)

C     iepsi gives the sign of the imaginary part of K
      call qlkfn(cxs,ieps,-si,m2,m4)
      xs=cxs(1)

C     Deal with non-singular special cases first
      if (qlzero(m2sqbar) .and. qlnonzero(m4sqbar)) then
         call qlratreal(m4*m2sqbar,m2*m4sqbar,yi,iepyi)
         cyi = dcmplx(yi)
         fac=xs/(cone-xs**2)/dcmplx((-m2*m4*ta))
         xlog=cln(xs,ieps)
         Ires(-2)=czip
         Ires(-1)=-xlog
         Ires( 0)=xlog*(-xlog-dcmplx(log(musq/m4sq))
     .   -2d0*qllnrat(m4sqbar,ta))
     .   -qlcLi2omx2(xs,xs,ieps,ieps)
     .   +qlcLi2omx2(xs,cyi,ieps,iepyi)
     .   -qlcLi2omx2(1d0/xs,cyi,-ieps,iepyi)
          goto 20
      elseif (qlzero(m4sqbar) .and. qlnonzero(m2sqbar)) then
         call qlratreal(m2*m4sqbar,m4*m2sqbar,yy,iepyy)
         cyy = dcmplx(yy)
         fac=xs/(cone-xs**2)/dcmplx((-m2*m4*ta))
         xlog=cln(xs,ieps)
         Ires(-2)=czip
         Ires(-1)=-xlog
         Ires( 0)=xlog*(-xlog-dcmplx(log(musq/m2sq))
     .   -2d0*qllnrat(m2sqbar,ta))
     .   -qlcLi2omx2(xs,xs,ieps,ieps)
     .   +qlcLi2omx2(xs,cyy,ieps,iepyy)
     .   -qlcLi2omx2(1d0/xs,cyy,-ieps,iepyy)
          goto 20
      elseif (qlzero(m4sqbar) .and. qlzero(m2sqbar)) then
          write(6,*) 'qlbox15:you got here in error'
          write(6,*) 'This is really qlbox14'
          write(6,*) 'qlbox15:m2sqbar,m4sqbar',m2sqbar,m4sqbar
          stop
      endif

      call qlratreal(m2*m4sqbar,m4*m2sqbar,yy,iepyy)
      rexs=dreal(xs)
      imxs=dimag(xs)
C----deal with s=(m2-m4)^2
      if ((qlzero(rexs-1d0)) .and. (qlzero(imxs))) then
          fac=dcmplx(-0.5d0/(m2*m4*ta))
          Ires(-2)=czip
          Ires(-1)=cone
          Ires(0)=dcmplx(log(musq/(m2*m4)))
     .    -qllnrat(m2sqbar,ta)-qllnrat(m4sqbar,ta)-dcmplx(2d0)
     .    -dcmplx((one+yy)/(one-yy))*qllnrat(m2*m4sqbar,m4*m2sqbar)
      else
C----deal with s .ne. (m2-m4)^2
          fac=xs/(cone-xs**2)/dcmplx((-m2*m4*ta))
          xlog=cln(xs,ieps)
          Ires(-2)=czip
          Ires(-1)=-xlog
          Ires( 0)=xlog*(-0.5d0*xlog-dcmplx(log(musq/(m2*m4)))
     .    -qllnrat(m2sqbar,ta)-qllnrat(m4sqbar,ta))
     .    -qlcLi2omx2(xs,xs,ieps,ieps)
     .    +chalf*qllnrat(m2*m4sqbar,m4*m2sqbar)**2
     .    +qlcLi2omx2(xs,dcmplx(yy),ieps,iepyy)
     .    +qlcLi2omx2(xs,dcmplx(1d0/yy),ieps,-iepyy)
      endif

 20   continue
      do iep=-2,0
      Ires(iep)=Ires(iep)*dcmplx(fac)
      enddo
      return
      end


subroutine setaux(mbc,mx,my,xlower,ylower,dx,dy,maux,aux)

    implicit none
    integer, intent(in) :: mbc,mx,my,maux
    real(kind=8), intent(in) :: xlower,ylower,dx,dy
    real(kind=8), intent(out) ::  aux(maux,1-mbc:mx+mbc,1-mbc:my+mbc)
    real(kind=8) :: xcell, ycell, cp, cs
    integer :: i,j

    real(kind=8) :: rho_plate, lambda_plate, mu_plate
    real(kind=8) :: rho_water, lambda_water, mu_water
    real(kind=8) :: rho_cell, lambda_cell, mu_cell

    ! Arrays to temporarily store computational and physical corners of grid cells
    real(kind=8) :: xccorn(4),yccorn(4),xpcorn(4),ypcorn(4)
    real(kind=8) :: norm, xn, yn, areap, b2c

    real (kind=8) :: center(2), theta, xcb(2), mindepth
    common /fault/  center, theta, xcb, mindepth

! c     #   (lambda = nu*E/((1+nu)(1-2nu))), E=young modulus, nu=poisson ration
! c     #   aux(1,i,j) is the density of the elastic material
! c     #   aux(2,i,j) is the lambda Lame parameter for elasticity
! c     #   aux(3,i,j) is the mu Lame parameter for elasticity (shear)
!
! c     #   aux(4,i,j) is cp is the P-wave speed (pressure wave)
! c     #   aux(5,i,j) is cs is the S-wave speed (shear wave)
!
! c     #   aux(6,i,j) is x component of normal at "left" boundary of grid point (i,j)
! c     #   aux(7,i,j) is y component of normal at "left" boundary of grid point (i,j)
! c     #   aux(8,i,j) =  is ratio of left edge to dy
!
! c     #   aux(9,i,j) is x component of normal at "bottom" boundary of grid point (i,j)
! c     #   aux(10,i,j) is y component of normal at "bottom" boundary of grid point (i,j)
! c     #   aux(11,i,j) =  is ratio of bottom edge to dx
!
! c     #   aux(12,i,j) = kappa  is ratio of cell area to dx*dy
!
! c     #   aux(13,i,j) = slip:
    !

    lambda_plate = 60.d9  ! Pa
    mu_plate = 30.d9      ! Pa
    rho_plate = 2500.d0   ! kg/m**3

    lambda_water = 2.202256d9  ! Pa
    mu_water = 0.d0      ! Pa
    rho_water = 1000.d0   ! kg/m**3

    ! Loop over all cells
      do j=1-mbc,my + mbc
        ycell = ylower + (j-0.5d0)*dy
        do i=1-mbc,mx + mbc

          if (ycell .gt. 0.d0) then
            lambda_cell = lambda_water
            mu_cell = mu_water
            rho_cell = rho_water
          else
            lambda_cell = lambda_plate
            mu_cell = mu_plate
            rho_cell = rho_plate
          endif

          aux(1,i,j) = rho_cell
          aux(2,i,j) = lambda_cell
          aux(3,i,j) = mu_cell

          ! Calculate pressure and shear wave speeds
          cp = dsqrt((lambda_cell + 2*mu_cell)/rho_cell)
          cs = dsqrt(mu_cell/rho_cell)
          aux(4,i,j) = cp
          aux(5,i,j) = cs

          ! ============================================
          !BEGINS CALCULATING AND SAVING NORMALS, LENGTH RATIOS AND AREA RATIO FOR MAPPED GRID

          ! Computes corners of grid cell
! c           # lower left corner:
          xccorn(1) = xlower + float(i-1)*dx
          yccorn(1) = ylower + float(j-1)*dy
          call mapc2p(xccorn(1),yccorn(1),xpcorn(1),ypcorn(1))

! c           # upper left corner:
          xccorn(2) = xccorn(1)
          yccorn(2) = yccorn(1) + dy
          call mapc2p(xccorn(2),yccorn(2),xpcorn(2),ypcorn(2))

! c           # upper right corner:
          xccorn(3) = xccorn(1) + dx
          yccorn(3) = yccorn(1) + dy
          call mapc2p(xccorn(3),yccorn(3),xpcorn(3),ypcorn(3))

! c           # lower right corner:
          xccorn(4) = xccorn(1) + dx
          yccorn(4) = yccorn(1)
          call mapc2p(xccorn(4),yccorn(4),xpcorn(4),ypcorn(4))

          ! Compute inner normals
          !left
          xn = ypcorn(2) - ypcorn(1)
          yn = -(xpcorn(2) - xpcorn(1))
          if (dabs(yn) < 1e-10) then
            yn = 0.0
          end if
          norm = dsqrt(xn*xn + yn*yn)
          aux(6,i,j) = xn/norm
          aux(7,i,j) = yn/norm
          aux(8,i,j) = norm/dy

          !bottom
          xn = -(ypcorn(4) - ypcorn(1))
          yn = xpcorn(4) - xpcorn(1)
          if (dabs(xn) < 1e-10) then
            xn = 0.0
          end if
          norm = dsqrt(xn*xn + yn*yn)
          aux(9,i,j) = xn/norm
          aux(10,i,j) = yn/norm
          aux(11,i,j) = norm/dx

          ! Computes area of grid cell using cross product of diagonal
          areap = (xpcorn(3) - xpcorn(1))*(ypcorn(2) - ypcorn(4))
          areap = areap - (ypcorn(3)-ypcorn(1))*(xpcorn(2)-xpcorn(4))
          areap = 0.5*dabs(areap)
          aux(12,i,j) = areap/(dx*dy)

          ! set slip:
          xccorn(1) = xlower + float(i-1)*dx
          yccorn(1) = ylower + float(j-1)*dy
          if ((abs(yccorn(1)-center(2)) < 0.5d0*dy) .and. &
              (xccorn(1) >= xcb(1)) .and. (xccorn(1) <= xcb(2))) then
                aux(13,i,j) = exp(-((xccorn(1)-center(1))/(center(1)-xcb(1)))**2)
            else
                aux(13,i,j) = 0.d0
            endif

        end do
      end do

end subroutine setaux