      SUBROUTINE TB01WX( COMPU, N, M, P, A, LDA, B, LDB, C, LDC, U, LDU,
     $                   DWORK, LDWORK, INFO )
C
C     SLICOT RELEASE 5.7.
C
C     Copyright (c) 2002-2020 NICONET e.V.
C
C     PURPOSE
C
C     To reduce the system state matrix A to an upper Hessenberg form
C     by using an orthogonal similarity transformation A <-- U'*A*U and
C     to apply the transformation to the matrices B and C: B <-- U'*B
C     and C <-- C*U.
C
C     ARGUMENTS
C
C     Mode Parameters
C
C     COMPU   CHARACTER*1
C             = 'N':  do not compute U;
C             = 'I':  U is initialized to the unit matrix, and the
C                     orthogonal matrix U is returned;
C             = 'U':  U must contain an orthogonal matrix U1 on entry,
C                     and the product U1*U is returned.
C
C     Input/Output Parameters
C
C     N       (input) INTEGER
C             The order of the original state-space representation,
C             i.e., the order of the matrix A.  N >= 0.
C
C     M       (input) INTEGER
C             The number of system inputs, or of columns of B.  M >= 0.
C
C     P       (input) INTEGER
C             The number of system outputs, or of rows of C.  P >= 0.
C
C     A       (input/output) DOUBLE PRECISION array, dimension (LDA,N)
C             On entry, the leading N-by-N part of this array must
C             contain the original state dynamics matrix A.
C             On exit, the leading N-by-N part of this array contains
C             the matrix U' * A * U in Hessenberg form. The elements
C             below the first subdiagonal are set to zero.
C
C     LDA     INTEGER
C             The leading dimension of the array A.  LDA >= MAX(1,N).
C
C     B       (input/output) DOUBLE PRECISION array, dimension (LDB,M)
C             On entry, the leading N-by-M part of this array must
C             contain the input matrix B.
C             On exit, the leading N-by-M part of this array contains
C             the transformed input matrix U' * B.
C
C     LDB     INTEGER
C             The leading dimension of the array B.  LDB >= MAX(1,N).
C
C     C       (input/output) DOUBLE PRECISION array, dimension (LDC,N)
C             On entry, the leading P-by-N part of this array must
C             contain the output matrix C.
C             On exit, the leading P-by-N part of this array contains
C             the transformed output matrix C * U.
C
C     LDC     INTEGER
C             The leading dimension of the array C.  LDC >= MAX(1,P).
C
C     U       (input/output) DOUBLE PRECISION array, dimension (LDU,*)
C             On entry, if COMPU = 'U', the leading N-by-N part of this
C             array must contain the given matrix U1. Otherwise, this
C             array need not be set on input.
C             On exit, if COMPU <> 'N', the leading N-by-N part of this
C             array contains the orthogonal transformation matrix used
C             to reduce A to the Hessenberg form (U1*U if COMPU = 'U').
C             If COMPU = 'N', this array is not referenced.
C
C     LDU     INTEGER
C             The leading dimension of the array U.
C             LDU >= 1,        if COMPU =  'N';
C             LDU >= max(1,N), if COMPU <> 'N'.
C
C     Workspace
C
C     DWORK   DOUBLE PRECISION array, dimension (LDWORK)
C             On exit, if INFO = 0, DWORK(1) returns the optimal value
C             of LDWORK.
C
C     LDWORK  INTEGER
C             The length of the array DWORK.  LDWORK >= 1, and if N > 0,
C             LDWORK >= N - 1 + MAX(N,M,P).
C             For optimum performance LDWORK should be larger.
C
C             If LDWORK = -1, then a workspace query is assumed; the
C             routine only calculates the optimal size of the DWORK
C             array, returns this value as the first entry of the DWORK
C             array, and no error message related to LDWORK is issued by
C             XERBLA.
C
C     Error Indicator
C
C     INFO    INTEGER
C             = 0:  successful exit;
C             < 0:  if INFO = -i, the i-th argument had an illegal
C                   value.
C
C     METHOD
C
C     Matrix A is reduced to the Hessenberg form using an orthogonal
C     similarity transformation A <- U'*A*U. Then, the transformation
C     is applied to the matrices B and C: B <-- U'*B and C <-- C*U.
C
C     NUMERICAL ASPECTS
C                                    3      2
C     The algorithm requires about 5N /3 + N (M+P) floating point
C                                              3
C     operations, if COMPU = 'N'. Otherwise, 2N /3 additional operations
C     are needed.
C
C     CONTRIBUTOR
C
C     A. Varga, German Aerospace Center,
C     DLR Oberpfaffenhofen, April 2002.
C
C     REVISIONS
C
C     V. Sima, Dec. 2016.
C
C     KEYWORDS
C
C     Orthogonal transformation, Hessenberg form, similarity
C     transformation.
C
C     ******************************************************************
C
C     .. Parameters ..
      DOUBLE PRECISION ZERO, ONE
      PARAMETER        ( ZERO = 0.0D0, ONE = 1.0D0 )
C     .. Scalar Arguments ..
      CHARACTER        COMPU
      INTEGER          INFO, LDA, LDB, LDC, LDU, LDWORK, M, N, P
C     .. Array Arguments ..
      DOUBLE PRECISION A(LDA,*), B(LDB,*), C(LDC,*), DWORK(*), U(LDU,*)
C     .. Local Scalars ..
      LOGICAL          ILU, LQUERY
      INTEGER          ICOMPU, ITAU, JWORK, MINWRK, WRKOPT
C     .. External Functions ..
      LOGICAL          LSAME
      EXTERNAL         LSAME
C     .. External Subroutines ..
      EXTERNAL         DGEHRD, DLACPY, DORGHR, DORMHR, XERBLA
C     .. Intrinsic Functions ..
      INTRINSIC        INT, MAX
C
C     .. Executable Statements ..
C
C     Decode COMPU.
C
      IF( LSAME( COMPU, 'N' ) ) THEN
         ILU = .FALSE.
         ICOMPU = 1
      ELSE IF( LSAME( COMPU, 'U' ) ) THEN
         ILU = .TRUE.
         ICOMPU = 2
      ELSE IF( LSAME( COMPU, 'I' ) ) THEN
         ILU = .TRUE.
         ICOMPU = 3
      ELSE
         ICOMPU = 0
      END IF
C
      INFO = 0
C
C     Check input parameters.
C
      IF( ICOMPU.LE.0 ) THEN
         INFO = -1
      ELSE IF( N.LT.0 ) THEN
         INFO = -2
      ELSE IF( M.LT.0 ) THEN
         INFO = -3
      ELSE IF( P.LT.0 ) THEN
         INFO = -4
      ELSE IF( LDA.LT.MAX( 1, N ) ) THEN
         INFO = -6
      ELSE IF( LDB.LT.MAX( 1, N ) ) THEN
         INFO = -8
      ELSE IF( LDC.LT.MAX( 1, P ) ) THEN
         INFO = -10
      ELSE IF( LDU.LT.1 .OR. ( ILU .AND. LDU.LT.MAX( 1, N ) ) ) THEN
         INFO = -12
      ELSE
         LQUERY = LDWORK.LT.0
         IF( N.EQ.0 ) THEN
            MINWRK = 1
         ELSE
            MINWRK = N - 1 + MAX( N, M, P )
         END IF
         IF( LQUERY ) THEN
            CALL DGEHRD( N, 1, N, A, LDA, DWORK, DWORK, -1, INFO )
            WRKOPT = MAX( MINWRK, N - 1 + INT( DWORK(1) ) )
            CALL DORMHR( 'Left', 'Transpose', N, M, 1, N, A, LDA,
     $                   DWORK, B, LDB, DWORK, -1, INFO )
            WRKOPT = MAX( WRKOPT, N - 1 + INT( DWORK(1) ) )
            CALL DORMHR( 'Right', 'No transpose', P, N, 1, N, A, LDA,
     $             DWORK, C, LDC, DWORK, -1, INFO )
            WRKOPT = MAX( WRKOPT, N - 1 + INT( DWORK(1) ) )
            IF( ILU ) THEN
               IF( ICOMPU.EQ.3 ) THEN
                  CALL DORGHR( N, 1, N, U, LDU, DWORK, DWORK, -1, INFO )
               ELSE
                  CALL DORMHR( 'Right', 'No transpose', N, N, 1, N, A,
     $                         LDA, DWORK, U, LDU, DWORK, -1, INFO )
               END IF
               WRKOPT = MAX( WRKOPT, N - 1 + INT( DWORK(1) ) )
            END IF
         ELSE IF( LDWORK.LT.MINWRK ) THEN
            INFO = -14
         END IF
      END IF
C
      IF( INFO.NE.0 ) THEN
C
C        Error return.
C
         CALL XERBLA( 'TB01WX', -INFO )
         RETURN
      ELSE IF( LQUERY ) THEN
         DWORK(1) = WRKOPT
         RETURN
      END IF
C
C     Quick return if possible.
C
      IF( N.EQ.0 ) THEN
         DWORK(1) = ONE
         RETURN
      END IF
C
C     Reduce A to Hessenberg form using an orthogonal similarity
C     transformation, A <- U'*A*U, and apply the orthogonal
C     transformations to B and C such that B <- U'*B, C <- C*U.
C
C     Workspace:  need   N-1+MAX(N,M,P);
C                 prefer N - 1 + MAX(N,M,P)*NB.
C
      ITAU  = 1
      JWORK = ITAU + N - 1
      CALL DGEHRD( N, 1, N, A, LDA, DWORK(ITAU), DWORK(JWORK),
     $             LDWORK-JWORK+1, INFO )
      WRKOPT = INT( DWORK(JWORK) )+JWORK-1
C
      CALL DORMHR( 'Left', 'Transpose', N, M, 1, N, A, LDA,
     $             DWORK(ITAU), B, LDB, DWORK(JWORK), LDWORK-JWORK+1,
     $             INFO )
      WRKOPT = MAX( WRKOPT, INT( DWORK(JWORK) )+JWORK-1 )
C
      CALL DORMHR( 'Right', 'No transpose', P, N, 1, N, A, LDA,
     $             DWORK(ITAU), C, LDC, DWORK(JWORK), LDWORK-JWORK+1,
     $             INFO )
      WRKOPT = MAX( WRKOPT, INT( DWORK(JWORK) )+JWORK-1 )
C
      IF( ILU ) THEN
         IF( ICOMPU.EQ.3 ) THEN
C
C           Accumulate the transformation in U.
C           Copy Householder vectors to U.
C
            CALL DLACPY( 'Lower', N, N, A, LDA, U, LDU )
C
C           Generate orthogonal matrix in U.
C           Workspace: need 2*N-1, prefer 2*N+(N-1)*NB.
C
            CALL DORGHR( N, 1, N, U, LDU, DWORK( ITAU ), DWORK(JWORK),
     $                   LDWORK-JWORK+1, INFO )
         ELSE
C
C           Apply the transformation to U1.
C
            CALL DORMHR( 'Right', 'No transpose', N, N, 1, N, A, LDA,
     $                   DWORK(ITAU), U, LDU, DWORK(JWORK),
     $                   LDWORK-JWORK+1, INFO )
         END IF
         WRKOPT = MAX( WRKOPT, INT( DWORK(JWORK) )+JWORK-1 )
      END IF
C
      IF( N.GT.2 )
     $   CALL DLASET( 'L', N-2, N-2, ZERO, ZERO, A(3,1), LDA )
C
      DWORK(1) = WRKOPT
C
      RETURN
C *** Last line of TB01WX ***
      END


      DOUBLE PRECISION FUNCTION DT_DSQEL_Q2(Jtyp,Enu,Q2)

C...differential cross section for  Quasi-Elastic scattering
C.       nu + N -> l + N'
C.  From Llewellin Smith  Phys.Rep.  3C, 261, (1971).
C.
C.  INPUT :  JTYP = 1,...,6    nu_e, ...., nubar_tau
C.           ENU (GeV) =  Neutrino energy
C.           Q2  (GeV**2) =  (Transfer momentum)**2
C.
C.  OUTPUT : DSQEL_Q2  = differential  cross section :
C.                       dsigma/dq**2  (10**-38 cm+2/GeV**2)
C------------------------------------------------------------------

      IMPLICIT NONE
      DOUBLE PRECISION a1 , a2 , aa , axial2 , bb , c0 , cc , csi ,
     &                 Enu , fa , fa0 , ffa , ffv1 , ffv2 , fv1 , fv2 ,
     &                 gve , gvm , Q2 , rm
      DOUBLE PRECISION ss , su , x , xa
      INTEGER Jtyp
      SAVE

C particle masses used in qel neutrino scattering modules
      INCLUDE 'inc/qnmass'
C*sr - removed (not needed)
C     COMMON /CAXIAL/ FA0, AXIAL2
C*

      DIMENSION ss(6)
      DATA c0/0.17590D0/    ! G_F**2 cos(theta_c)**2 M**2 /(8 pi) 10**-38 cm+2
      DATA ss/1.D0 , -1.D0 , 1.D0 , -1.D0 , 1.D0 , -1.D0/
      DATA axial2/1.03D0/   ! to be checked

      fa0 = -1.253D0
      csi = 3.71D0                   !  ???
      gve = 1.D0/(1.D0+Q2/0.84D0**2)**2      ! G_e(q**2)
      gvm = (1.D0+csi)*gve           ! G_m (q**2)
      x = Q2/(EMN*EMN)     ! emn=massa barione
      xa = x/4.D0
      fv1 = 1.D0/(1.D0+xa)*(gve+xa*gvm)
      fv2 = 1.D0/(1.D0+xa)*(gvm-gve)
      fa = fa0/(1.D0+Q2/axial2)**2
      ffa = fa*fa
      ffv1 = fv1*fv1
      ffv2 = fv2*fv2
      rm = EMLsq(Jtyp)/(EMN*EMN)            ! emlsq(jtyp)
      a1 = (4.D0+x)*ffa - (4.D0-x)*ffv1 + x*ffv2*(1.D0-xa) + 4*x*fv1*fv2
      a2 = -rm*((fv1+fv2)**2+ffa)
      aa = (xa+0.25D0*rm)*(a1+a2)
      bb = -x*fa*(fv1+fv2)
      cc = 0.25D0*(ffa+ffv1+xa*ffv2)
      su = (4.D0*Enu*EMN-Q2-EMLsq(Jtyp))/(EMN*EMN)
      DT_DSQEL_Q2 = c0*(aa+ss(Jtyp)*bb*su+cc*su*su)/(Enu*Enu)        !
      IF ( DT_DSQEL_Q2.LT.0.D0 ) DT_DSQEL_Q2 = 0.D0

      END FUNCTION


      SUBROUTINE ROOT34(C,KDEG,ROOTS,NREAL)
      IMPLICIT REAL*8 (A-H,O-Z)
C
C  THIS ROUTINE FINDS THE ROOTS OF CUBIC AND QUARTIC EQUATIONS.
C
C
C  VAR    DIM      TYPE  I/O  DESCRIPTION
C  ---    ---      ----  ---  -----------
C
C  C      KDEG+1   R*8    I   COEFFICIENTS IN EQUATION:
C                             0.0=C(1)+C(2)*X+C(3)*X**2+C(4)*X**3, OR
C                             0.0=C(1)+....+C(5)*X**4
C
C  KDEG   1        I*4    I   DEGREE OF THE POLYNOMIAL FOR WHICH REAL
C                             ROOTS ARE SOUGHT. =3 FOR CUBICS, =4 FOR
C                             QUARTICS. ERROR OTHERWISE.
C
C  ROOTS  NDEG     R*8    O   REAL ROOTS OF THE ABOVE EQUATIONS.
C
C  NREAL  1        I*4    O   NUMBER OF REAL ROOTS FOUND. THESE ARE
C                             LOCATED IN ROOTS(1),...,ROOTS(NREAL)
C
C***********************************************************************
C
C     WRITTEN BY RON COOK OF CSC. GIVEN TO CJP 7/77.
C     TRANSFERRED TO VAX 9/81. C PETRUZZO
C
C***********************************************************************
C
      REAL*8 C(1),ROOTS(1)
      REAL*8 AI(4)
      EQUIVALENCE  (AI(1),R),        (AI(2),Q),        (AI(3),P)
      DATA     S3 / 1.7320508D0/
C
      NS=0
      IF(KDEG.EQ.3.OR.KDEG.EQ.4) GO TO 300
C     ERROR. CANNOT USE THIS ROUTINE EXCEPT FOR CUBICS AND QUARTICS.
      NREAL=-IABS(KDEG)
      GO TO 999
C
  300 CONTINUE
      DO 1 I=1,KDEG
    1 AI(I) = C(I)/C(KDEG+1)
      IF(KDEG.EQ.3) GO TO 3
C     SET UP FOR QUARTIC.
      H =-AI(4)/4.D0
      U = H*(6.D0*H+3.D0*AI(4))+AI(3)
      V = H*(H*(4.D0*H+3.D0*AI(4))+2.D0*AI(3))+AI(2)
      W = H*(H*(H*(H+AI(4))+AI(3))+AI(2))+AI(1)
      P = 2.D0*U
      Q = U**2-4.D0*W
      R =-V**2
    3 CONTINUE
      P2=P*P
      A = (3.D0*Q-P2)/3.D0
      B = P*(2.D0*P2-9.D0*Q)/27.D0+R
      S =-P/3.D0
      A327=A*A*A/27.D0
      BO2=B/2.D0
      DEL=A327+BO2*BO2
      IF(DEL) 10,20,30
   10 CONTINUE
      E0 = 2.D0 * DSQRT(-A/3.D0)
      CO = -BO2/DSQRT(-A327)
      SO = DSQRT(1.D0-CO*CO)
      O = DATAN2(SO,CO)
      OO3=O/3.D0
      COO3 = DCOS(OO3)
      SOO3 = DSIN(OO3)
      Z1=E0*COO3
      Z2=-E0*(COO3+S3*SOO3)/2.D0
      Z3=-E0*(COO3-S3*SOO3)/2.D0
      NS = 3
      GO TO 40
   20 CONTINUE
      IF (BO2 .LT. 0.D0) GO TO 30
      Z1 = BO2**(1.D0/3.D0)
      GO TO 31
   30 CONTINUE
      SRD = DSQRT(DEL)
      CURT=1.D0/3.D0
      FAC1 = -BO2 +SRD
      FAC2 = -BO2 -SRD
      Z1=DSIGN(DABS(FAC1)**CURT,FAC1)+DSIGN(DABS(FAC2)**CURT,FAC2)
31    Z2 = Z1 -1.D0
      Z3=Z2
      NS = 1
   40 CONTINUE
      Z1=Z1+S
      Z2=Z2+S
      Z3=Z3+S
      IF(KDEG.EQ.4) GO TO 45
      ROOTS(1) = Z1
      ROOTS(2) = Z2
      ROOTS(3) = Z3
      GO TO 80
   45 CONTINUE
      NS = 0
      RP=DMAX1(Z1,Z2,Z3)
      SR = DSQRT(RP)
      SRO2=SR/2.D0
      PPRP = (U+RP)/2.D0
      QOSR = V/SR/2.D0
      ROOTSI = PPRP-QOSR
      BETA = PPRP+QOSR
      DISC=RP-4.D0*ROOTSI
      IF(DISC) 60,50,50
   50 CONTINUE
      DISC = DSQRT (DISC)/2.D0
      ROOTS(1)=-SRO2+DISC+H
      ROOTS(2)=-SRO2-DISC+H
      NS = 2
   60 CONTINUE
      DISC=RP-4.D0*BETA
      IF(DISC) 80,70,70
   70 CONTINUE
      DISC = DSQRT(DISC)/2.D0
      ROOTS(NS+1) = SRO2 + DISC + H
      NS = NS + 2
      ROOTS(NS) = SRO2 - DISC + H
   80 CONTINUE
      NREAL = NS
  999 CONTINUE
      RETURN
      END


      FUNCTION BESRAT(V)
C ----------------------------------------------------------------
C RETURNS BESRAT = A(K) FOR K = ABS(V), WHERE A(K) IS THE EXPECTED
C MODULUS OF THE MEAN VECTOR SUM OF UNIT VECTORS SAMPLED FROM THE
C VON MISES DISTRIBUTION OF DIRECTIONS IN 2D WITH PARAMETER = K.
C A(V) = THE RATIO OF MODIFIED BESSEL FUNCTIONS OF THE FIRST KIND
C OF ORDERS 1 AND 0, I.E., A(V) = I1(V)/I0(V).
C ----------------------------------------------------------------
C
C  ADJUST TO S DECIMAL DIGIT PRECISION BY SETTING DATA CONSTANTS -
C     C1 = (S+9.0-8.0/S)*0.0351
C     C2 = ((S-5.0)**3/180.0+S-5.0)/10.0
C     CX = S*0.5 + 11.0
C  FOR S IN RANGE (5,14).  THUS FOR S = 9.3 :
      DATA C1 /0.613/, C2 /0.475/, CX /15.65/
C
      Y = 0.0
      X = ABS(V)
      IF (X.GT.CX) GO TO 20
C
C  FOR SMALL X, RATIO = X/(2+X*X/(4+X*X/(6+X*X/(8+ ... )))
      N = INT((X+16.0-16.0/(X+C1+0.75))*C1)
      X = X*0.5
      XX = X*X
      DO 10 J=1,N
        Y = XX/(FLOAT(N-J+2)+Y)
   10 CONTINUE
      BESRAT = X/(1.0+Y)
      RETURN
C
C  FOR LARGE X, RATIO = 1-2/(4X-1-1/(4X/3-2-1/(4X/5-2- ... )))
   20 N = INT((68.0/X+1.0)*C2) + 1
      X = X*4.0
      XX = FLOAT(N*2+1)
      DO 30 J=1,N
        Y = XX/((-2.0-Y)*XX+X)
        XX = XX - 2.0
   30 CONTINUE
      BESRAT = 1.0 - 2.0/(X-1.0-Y)
      RETURN
      END


***********************************************************************
c 	               ORBEL_XV2EL.F
***********************************************************************
*     PURPOSE:  Given the cartesian position and velocity of an orbit,
*       compute the osculating orbital elements.
*
C       input:
c            x,y,z    ==>  position of object (real scalars)
c            vx,vy,vz ==>  velocity of object (real scalars)
c            gmsum       ==> G*(M1+M2) (real scalar)
c
c       Output:
c	     ialpha   ==> conic section type ( see PURPOSE, integer scalar)
C	     a        ==> semi-major axis or pericentric distance if a parabola
c                          (real scalar)
c            e        ==> eccentricity (real scalar)
C            inc      ==> inclination  (real scalar)
C            capom    ==> longitude of ascending node (real scalar)
C	     omega    ==> argument of perihelion (real scalar)
C	     capm     ==> mean anomoly(real scalar)
c
*     ALGORITHM: See e.g. p.70 of Fitzpatrick's "Priciples of Cel. Mech."
*     REMARKS:  If the inclination INC is less than TINY, we
*       arbitrarily choose the longitude of the ascending node LGNODE
*       to be 0.0 (so the ascending node is then along the X axis).  If
*       the  eccentricity E is less than SQRT(TINY), we arbitrarily
*       choose the argument of perihelion to be 0.
*     AUTHOR:  M. Duncan.
*     DATE WRITTEN:  May 8,1992.
*     REVISIONS: 1/7/97
***********************************************************************

 	subroutine orbel_xv2el(x,y,z,vx,vy,vz,gmsum,
     &     ialpha,a,e,inc,capom,omega,capm)

      include '../swift.inc'

c...  Inputs Only:
	real*8 x,y,z,vx,vy,vz,gmsum

c...  Outputs
	integer ialpha
        real*8 a,e,inc,capom,omega,capm

c...  Internals:
        real*8 hx,hy,hz,h2,h,r,v2,v,vdotr,energy,fac,face,cape,capf,tmpf
	real*8 cw,sw,w,u

c----
c...  Executable code

* Compute the angular momentum H, and thereby the inclination INC.

	hx = y*vz - z*vy
	hy = z*vx - x*vz
	hz = x*vy - y*vx
	h2 = hx*hx + hy*hy +hz*hz
	h  = sqrt(h2)
	inc = acos(hz/h)

* Compute longitude of ascending node CAPOM and the argument of
* latitude u.
	fac = (hx**2 + hy**2)/h2

	if(fac.lt. TINY ) then
	  capom = 0.d0
	  u = atan2(y,x)
	  if(abs(inc - PI).lt. 10.d0*TINY) u = -u
	else
	  capom = atan2(hx,-hy)
	  u = atan2 ( z/sin(inc) , x*cos(capom) + y*sin(capom))
	endif

	if(capom .lt. 0.d0) capom = capom + 2.d0*PI
	if(u .lt. 0.d0) u = u + 2.d0*PI

*  Compute the radius R and velocity squared V2, and the dot
*  product RDOTV, the energy per unit mass ENERGY .

	r = sqrt(x*x + y*y + z*z)
	v2 = vx*vx + vy*vy + vz*vz
	v = sqrt(v2)
	vdotr = x*vx + y*vy + z*vz
	energy = 0.5d0*v2 - gmsum/r

*  Determine type of conic section and label it via IALPHA
	if(abs(energy*r/gmsum) .lt. sqrt(TINY)) then
	   ialpha = 0
	else
	   if(energy .lt. 0.d0) ialpha = -1
	   if(energy .gt. 0.d0) ialpha = +1
	endif

* Depending on the conic type, determine the remaining elements

***
c ELLIPSE :
	if(ialpha .eq. -1) then
	  a = -0.5d0*gmsum/energy
	  fac = 1.d0 - h2/(gmsum*a)

          if (fac .gt. TINY) then
             e = sqrt ( fac )
             face =(a-r)/(a*e)

c... Apr. 16/93 : watch for case where face is slightly outside unity
             if ( face .gt. 1.d0) then
                cape = 0.d0
             else
                if ( face .gt. -1.d0) then
                   cape = acos( face )
                else
                   cape = PI
                endif
             endif

            if ( vdotr .lt. 0.d0 ) cape = 2.d0*PI - cape
	    cw = (cos( cape) -e)/(1.d0 - e*cos(cape))
	    sw = sqrt(1.d0 - e*e)*sin(cape)/(1.d0 - e*cos(cape))
	    w = atan2(sw,cw)
	    if(w .lt. 0.d0) w = w + 2.d0*PI
	  else
	    e = 0.d0
	    w = u
	    cape = u
	  endif

	  capm = cape - e*sin (cape)
	  omega = u - w
	  if(omega .lt. 0.d0) omega = omega + 2.d0*PI
	  omega = omega - int(omega/(2.d0*PI))*2.d0*PI

	endif
***
***
c HYPERBOLA
	if(ialpha .eq. +1) then

	  a = +0.5d0*gmsum/energy
	  fac = h2/(gmsum*a)

          if (fac .gt. TINY) then
 	    e = sqrt ( 1.d0 + fac )
	    tmpf = (a+r)/(a*e)
            if(tmpf.lt.1.0d0) then
               tmpf = 1.0d0
            endif
	    capf = log(tmpf + sqrt(tmpf*tmpf -1.d0))
	    if ( vdotr .lt. 0.d0 ) capf = - capf
	    cw = (e - cosh(capf))/(e*cosh(capf) - 1.d0 )
	    sw = sqrt(e*e - 1.d0)*sinh(capf)/(e*cosh(capf) - 1.d0 )
	    w = atan2(sw,cw)
	    if(w .lt. 0.d0) w = w + 2.d0*PI
	  else
c we only get here if a hyperbola is essentially a parabola
c so we calculate e and w accordingly to avoid singularities
	    e = 1.d0
	    tmpf = 0.5d0*h2/gmsum
	    w = acos(2.d0*tmpf/r -1.d0)
	    if ( vdotr .lt. 0.d0) w = 2.d0*PI - w
	    tmpf = (a+r)/(a*e)
	    capf = log(tmpf + sqrt(tmpf*tmpf -1.d0))
	  endif

	  capm = e * sinh(capf) - capf
	  omega = u - w
	  if(omega .lt. 0.d0) omega = omega + 2.d0*PI
	  omega = omega - int(omega/(2.d0*PI))*2.d0*PI
	endif
***
***
c PARABOLA : ( NOTE - in this case we use "a" to mean pericentric distance)
	if(ialpha .eq. 0) then
	  a =  0.5d0*h2/gmsum
	  e = 1.d0
	  w = acos(2.d0*a/r -1.d0)
	  if ( vdotr .lt. 0.d0) w = 2.d0*PI - w
	  tmpf = tan(0.5d0 * w)
	  capm = tmpf* (1.d0 + tmpf*tmpf/3.d0)
	  omega = u - w
	  if(omega .lt. 0.d0) omega = omega + 2.d0*PI
	  omega = omega - int(omega/(2.d0*PI))*2.d0*PI
	endif
***
***
	return
	end    ! orbel_xv2el
c------------------------------------------------------------------


SUBROUTINE DKOGH2(UPPER, F, G, H, TOL, RHSIDE, CU, SU, CV, SV, F1, H1, INFO)
  IMPLICIT NONE

  LOGICAL, INTENT(IN) :: UPPER
  DOUBLE PRECISION, INTENT(IN) :: F, G, H, TOL

  LOGICAL, INTENT(OUT) :: RHSIDE
  DOUBLE PRECISION, INTENT(OUT) :: CU, SU, CV, SV, F1, H1
  INTEGER, INTENT(OUT) :: INFO

  DOUBLE PRECISION :: TH2, TH, TG, Q

  INFO = 0
  RHSIDE = .TRUE.

  IF (UPPER) THEN
     TH2 = (TWO * F * G) / (F * F + G * G + H * H)
     TH = TH2 / (ONE + SQRT(C_FMA(-TH2, TH2, ONE)))
     TG = C_FMA(F, TH, -G) / H
     CU = ONE / SQRT(C_FMA(TG, TG, ONE))
     SU = CU * TG
     CV = ONE / SQRT(C_FMA(-TH, TH, ONE))
     SV = CV * TH

     Q = CU / CV
     F1 = F * Q
     H1 = H / Q
  ELSE
     TH2 = (TWO * G * H) / (F * F + G * G + H * H)
     TH = TH2 / (ONE + SQRT(C_FMA(-TH2, TH2, ONE)))
     TG = C_FMA(-H, TH, G) / F
     CU = ONE / SQRT(C_FMA(TG, TG, ONE))
     SU = CU * TG
     CV = ONE / SQRT(C_FMA(-TH, TH, ONE))
     SV = CV * TH

     Q = CU / CV
     F1 = F / Q
     H1 = H * Q
  END IF
END SUBROUTINE DKOGH2


      SUBROUTINE CALBAL1
C
C CHANGE RECORD
C **  SUBROUTINES CALBAL CALCULATE GLOBAL VOLUME, MASS, MOMENTUM,
C **  AND ENERGY BALANCES
C
      USE GLOBAL
	IMPLICIT NONE
	INTEGER::L,LN,K
      IF(NBAL.GT.1) RETURN
C
C **  INITIALIZE VOLUME, SALT MASS, DYE MASS, MOMENTUM, KINETIC ENERGY
C **  AND POTENTIAL ENERGY, AND ASSOCIATED FLUXES
C
      VOLBEG=0.
      SALBEG=0.
      DYEBEG=0.
      UMOBEG=0.
      VMOBEG=0.
      UUEBEG=0.
      VVEBEG=0.
      PPEBEG=0.
      BBEBEG=0.
      VOLOUT=0.
      SALOUT=0.
      DYEOUT=0.
      UMOOUT=0.
      VMOOUT=0.
      UUEOUT=0.
      VVEOUT=0.
      PPEOUT=0.
      BBEOUT=0.
      DO L=2,LA
        LN=LNC(L)
        VOLBEG=VOLBEG+SPB(L)*DXYP(L)*HP(L)
        UMOBEG=UMOBEG+SPB(L)*0.5*DXYP(L)*HP(L)*(DYIU(L)*HUI(L)*UHDYE(L)
     &      +DYIU(LEAST(L))*HUI(LEAST(L))*UHDYE(LEAST(L)))
        VMOBEG=VMOBEG+SPB(L)*0.5*DXYP(L)*HP(L)*(DXIV(L)*HVI(L)*VHDXE(L)
     &      +DXIV(LN)*HVI(LN)*VHDXE(LN))
        PPEBEG=PPEBEG+SPB(L)*0.5*DXYP(L)
     &      *(GI*P(L)*P(L)-G*BELV(L)*BELV(L))
      ENDDO
      AMOBEG=SQRT(UMOBEG*UMOBEG+VMOBEG*VMOBEG)
      DO K=1,KC
        DO L=2,LA
          LN=LNC(L)
          SALBEG=SALBEG+SCB(L)*DXYP(L)*HP(L)*SAL(L,K)*DZC(K)
          DYEBEG=DYEBEG+SCB(L)*DXYP(L)*HP(L)*DYE(L,K)*DZC(K)
          UUEBEG=UUEBEG+SPB(L)*0.125*DXYP(L)*HP(L)*DZC(K)
     &        *( (U(L,K)+U(LEAST(L),K))*(U(L,K)+U(LEAST(L),K)) )
          VVEBEG=VVEBEG+SPB(L)*0.125*DXYP(L)*HP(L)*DZC(K)
     &        *( (V(L,K)+V(LN,K))*(V(L,K)+V(LN,K)) )
          BBEBEG=BBEBEG+SPB(L)*GP*DXYP(L)*HP(L)*DZC(K)*( BELV(L)
     &        +0.5*HP(L)*(Z(K)+Z(K-1)) )*B(L,K)
        ENDDO
      ENDDO
      RETURN
      END


#include "Config.fpp"
module mod_sawtooth_signal

 !===================================================================================85
 !---------------------------- DESCRIPTION ------------------------------------------85
 !
 !
 !
 !          Module  name:
 !                         'mod_sawtooth_signal'
 !
 !          Purpose:
 !                   Sawtooth Signal representation
 !                   approximated by Fourier series
 !                   and additionally reprsented by
 !
 !          History:
 !                        Date: 14-08-2017
 !                        Time: 15:41 GMT+2
 !           Modified:
 !                     By Bernard Gingold on: 09-03-2018, 17:33 GMT+2
 !                   1) Refactoring to Fortran 2003 standard.
 !                   2) Removing unnecessary type-bound members
 !                   3) Adding computational subroutines (additive noise)
 !                   4) Removing unnecessary destructor,copy and initialization subroutines.
 !          Version:
 !
 !                      Major: 1
 !                      Minor: 0
 !                      Micro: 0
 !
 !          Author:  Bernard Gingold
 !
 !
 !
 !
 !
 !          E-mail:
 !
 !                      beniekg@gmail.com
 !==================================================================================85

 ! Tab:5 col - Type and etc.. definitions
 ! Tab:10,11 col - Type , function and subroutine code blocks.

    use module_kinds,        only : I32P, R64P
    use mod_code_timing
    use mod_complex_arithm , only : vcmag
    use mod_jonesvec
    implicit none

    !=====================================================59
    !  File and module information:
    !  version,creation and build date, author,description
    !=====================================================59

    ! Major version
    integer(I32P), parameter, public :: MOD_SAWTOOTH_SIGNAL_MAJOR = 1

    ! Minor version
    integer(I32P), parameter, public :: MOD_SAWTOOTH_SIGNAL_MINOR = 0

    ! Micro version
    integer(I32P), parameter, public :: MOD_SAWTOOTH_SIGNAL_MICRO = 0

    ! Module full version
    integer(I32P), parameter, public :: MOD_SAWTOOTH_SIGNAL_FULLVER = 1000*MOD_SAWTOOTH_SIGNAL_MAJOR+100*MOD_SAWTOOTH_SIGNAL_MINOR + &
                                                                      10*MOD_SAWTOOTH_SIGNAL_MICRO

    ! Module creation date
    character(*),  parameter, public :: MOD_SAWTOOTH_SIGNAL_CREATE_DATE = "14-08-2017 15:59 +00200 (MON 14 AUG 2017 GMT+2)"

    ! Module build date (should be set after every successful build)
    character(*),  parameter, public :: MOD_SAWTOOTH_SIGNAL_BUILD_DATE = " "

    ! Module author info
    character(*),  parameter, public :: MOD_SAWTOOTH_SIGNAL_AUTHOR = "Programmer: Bernard Gingold e-mail: beniekg@gmail.com"

    ! Module short description
    character(*),  parameter, public :: MOD_SAWTOOTH_SIGNAL_DESCRIPT = "Sawtooth signal approximated by Fourier Series."

    !======================================================60
    !  Type: SawtoothSignal_t
    !======================================================60

    type, public :: SawtoothSignal_t

        private

        ! Signal name
        character(len=64)   :: m_name

        ! Signal ID
        integer(I32P)       :: m_sid

        ! Number of signal samples
        integer(I32P)       :: m_nsamp

        ! Number of Fourier series
        integer(I32P)       :: m_maxk

        ! Signal duration
        real(R64P)          :: m_dur

        ! Sawtooth amplitude
        real(R64P)          :: m_A

        ! Sawtooth initial time
        real(R64P)          :: m_sinit

        ! Sawtooth increment time-step
        real(R64P)          :: m_stsinc

        ! Sawtooth frequency
        real(R64P)          :: m_freq

        ! Jones Vector
        type(JonesVector_t), allocatable, dimension(:),   public :: m_jvec

        ! Sine components of K sinusoids
        real(R64P), allocatable, dimension(:,:),          public :: m_scomp
!DIR$   ATTRIBUTES ALIGN : 64 :: m_scomp

        ! Cosine components of K cosinusoids
        real(R64P), allocatable, dimension(:,:),          public :: m_ccomp
!DIR$   ATTRIBUTES ALIGN : 64 :: m_ccomp

        ! Sawtooth carrier frequency*t
        real(R64P), allocatable, dimension(:),            public   :: m_wct
!DIR$   ATTRIBUTES ALIGN : 64 :: m_phi

        ! Signal natural envelope
        real(R64P), allocatable, dimension(:),            public   :: m_nenvp
!DIR$   ATTRIBUTES ALIGN : 64 :: m_nenvp

        ! Signal  i.e. Sawtooth(phi(t))
        ! approximated by K sinusoids
        real(R64P), allocatable, dimension(:),            public   :: m_sawtooth
!DIR$   ATTRIBUTES ALIGN : 64 :: m_sawtooth

        ! Sawtooth complex representation
        complex(R64P), allocatable, dimension(:),         public   :: m_cform
!DIR$   ATTRIBUTES ALIGN : 64 :: m_canform

        ! Electric field
        complex(R64P), allocatable, dimension(:),         public   :: m_E
!DIR$   ATTRIBUTES ALIGN : 64 :: m_E

         ! Signal amplitude  A(phi,theta)/r used for calculation electric field
         ! far from emmiter(antenna)
         real(R64P), allocatable, dimension(:),           public   :: m_amp
!DIR$   ATTRIBUTES ALIGN : 64 :: m_amp

 ! Time-averaged power density

         complex(R64P), allocatable, dimension(:),        public    :: m_S
!DIR$   ATTRIBUTES ALIGN : 64 :: m_S

        contains

        ! Creates Sawtooth signal approximated by its
        ! Fourier series
        procedure, pass(this), public :: sawtooth_signal

        !
        ! Creates signal with noise modulated phase
        procedure, pass(this), public :: phase_noise_sawtooth_signal

        ! Creates signal with noise modulated phase
        ! coupled with background additive noise.
        procedure, pass(this), public :: additive_noise_sawtooth_signal

        !==========================================52
        !   Getter pure functions
        !==========================================52

        procedure, pass(this), public :: get_name

        procedure, pass(this), public :: get_sid

        procedure, pass(this), public :: get_nsamp

        procedure, pass(this), public :: get_maxk

        procedure, pass(this), public :: get_dur

        procedure, pass(this), public :: get_A

        procedure, pass(this), public :: get_sinit

        procedure, pass(this), public :: get_stsinc

        procedure, pass(this), public :: get_freq

        !==================================================60
        ! Computational subroutines
        !==================================================60

        procedure, pass(this), public :: dphi_dt

        !==============================================56
        !      write/read subroutines
        !==============================================56
        procedure, nopass, public :: write_state

        procedure, nopass, public :: read_state
        !==============================================56
        !   Class helper procedures
        !==============================================56

        procedure, pass(this),  public :: dbg_info

        !==============================================56
        !    Generic operators
        !==============================================56

        procedure, public :: copy_assign

        generic :: assignment (=) => copy_assign

    end type SawtoothSignal_t

    interface SawtoothSignal_t
        procedure :: constructor
    end interface SawtoothSignal_t

    contains
               ! type(SawtoothSignal_t) :: sig
               ! sig = SawtoothSignal_t(1,2,3,4)
               ! call sig%sawtooth_signal(...,..,..
    !======================================================60
    ! @function: constructor
    !             default initialization (no physical meaning)
    !======================================================60
    type(SawtoothSignal_t) function constructor(name,sid,nsamp,maxk,logging, &
                                                verbose, filename,append  )
          use mod_print_error,  only : handle_fatal_memory_error
          use mod_constants,    only : LAM_PINF, LAM_MINSAMP, &
                                       LAM_MINK
          character(len=*),        intent(in)    :: name
          integer(I32P),           intent(in)    :: sid
          integer(I32P),           intent(inout) :: nsamp,maxk
          logical(I32P),           intent(in)    :: logging,verbose
          character(len=*),        intent(in)    :: filename
          logical(I32P),           intent(in)    :: append
          ! locals
          character(len=256) :: emsg
          integer(I32P)      :: aerr
          ! Start of executable statements
          if(nsamp <  LAM_MINSAMP) then
              nsamp = LAM_MINSAMP
          end if
          if(maxk <  LAM_MINK) then
              maxk = LAM_MINK
          end if
           ! Begin construction
          constructor%m_name   = name
          constructor%m_sid    = sid
          constructor%m_nsamp  = nsamp
          constructor%m_maxk   = maxk
          constructor%m_dur    = LAM_PINF
          constructor%m_A      = LAM_PINF
          constructor%m_sinit  = LAM_PINF
          constructor%m_stsinc = LAM_PINF
          constructor%m_freq   = LAM_PINF
          associate(k=>constructor%m_maxk,        &
                    n=>constructor%m_nsamp)
              allocate(constructor%m_jvec(n),     &
                       constructor%m_scomp(k,n),  &
                       constructor%m_ccomp(k,n),  &
                       constructor%m_wct(n),      &
                       constructor%m_nenvp(n),    &
                       constructor%m_sawtooth(n), &
                       constructor%m_cform(n),    &
                       constructor%m_E(n),        &
                       constructor%m_amp(n),      &
                       constructor%m_S(n),        &
                       STAT=aerr,                 &
                       ERRMSG=emsg )
          end associate
          if(aerr /= 0) then

              call handle_fatal_memory_error( logging,verbose,append,filename,   &
                                              "logger:319 --> mod_sawtooth_signal/constructor: Memory Allocation Failure!!" , &
                                              "mod_sawtooth_signal/constructor:319 -- Memory Allocation Failure!" ,  &
                                              emsg,__LINE__ )
          end if
          constructor%m_jvec     = JonesVector()
          constructor%m_scomp    = LAM_PINF
          constructor%m_ccomp    = LAM_PINF
          constructor%m_wct      = LAM_PINF
          constructor%m_nenvp    = LAM_PINF
          constructor%m_sawtooth = LAM_PINF
          constructor%m_cform    = CMPLX(LAM_PINF,LAM_PINF,R64P)
          constructor%m_E        = CMPLX(LAM_PINF,LAM_PINF,R64P)
          constructor%m_amp      = LAM_PINF
          constructor%m_S        = CMPLX(LAM_PINF,LAM_PINF,R64P)
   end function constructor

    !======================================================60
    !  subroutine: sawtooth_signal
    !              Physical representation of  Sawtooth
    !              approximated by K sinusoids (Fourier-
    !              series)
    !======================================================60
    subroutine sawtooth_signal(this,ierr,dur,A,                      &
                             sinit,stsinc,freq,r,h,v,                &
                             verbose,logging,filename,append,        &
                              profiling,qpctimer      )
          use mod_print_error,   only : handle_fatal_memory_error
          use mod_constants,     only : LAM_ZR8, LAM_HR64P, LAM_2PI,LAM_PI, LAM_PINF
          class(SawtoothSignal_t),     intent(inout) :: this
          integer(I32P),               intent(inout) :: ierr
          real(R64P),                  intent(inout) :: dur,A
          real(R64P),                  intent(in)    :: sinit,stsinc,freq
          real(R64P), dimension(:),    intent(in)    :: r
!DIR$     ASSUME_ALIGNED r:64
          complex(R64P), dimension(:), intent(in)    :: h,v
!DIR$     ASSUME_ALIGNED h:64,v:64
          logical(I32P),               intent(in)    :: verbose,logging
          character(len=*),            intent(in)    :: filename
          logical(I32P),               intent(in)    :: append,profiling
          type(QPCTimer_t),            intent(inout) :: qpctimer
          ! Locals
          character(len=256) :: emsg
          integer(I32P)      :: k,j,aerr
          integer(BOOL)      :: ifail
          logical(I32P)      :: bfail
          real(R64P)         :: insamp,t,t2,delta,hA,API
          real(R64P),    allocatable, dimension(:) :: tcos,ts,tc
!DIR$     ATTRIBUTES ALIGN : 64 ::  tcos
!DIR$     ATTRIBUTES ALIGN : 64 ::  ts
!DIR$     ATTRIBUTES ALIGN : 64 ::  tc
          ! Start of executable statements
          if(ierr < 0_I32P) ierr = 0_I32P
          if(dur <=   LAM_ZR8 .OR. &
             A   <=   LAM_ZR8 ) then
              ierr = -1_I32P
              return
          end if
          this%m_dur = dur
          this%m_A  = A
          this%m_sinit = sinit
          this%m_stsinc = stsinc
          this%m_freq = freq
          associate(n=>this%m_nsamp)
              allocate(tcos(n),             &
                       ts(n),               &
                       tc(n),               &
                       STAT=aerr,           &
                       ERRMSG=emsg         )
        end associate
        if(aerr /= 0) then

              call handle_fatal_memory_error( logging,verbose,append,filename,    &
                                              "logger:397 --> mod_sawtooth_singnal/sawtooth_signal: Memory Allocation Failure!!", &
                                              "mod_sawtooth_signal/sawtooth_signal:397 --  Memory Allocation Failure!" ,          &
                                              emsg,__LINE__ )
        end if
        tcos = LAM_PINF
        ts   = LAM_PINF
        tc   = LAM_PINF
        if(profiling) then
              call qpctimer_start(qpctimer,ifail)
              if(ifail == 0) then
                  write(stderr,*) "mod_sawtooth_signal/sawtooth_signal: qpctimer_start failed to query performance frequency counter!"
              end if
        end if
          ! Set up jones vector field
        do j = 1, this%m_nsamp
              this%m_jvec(j) = JonesVector_t(h(j),v(j))
        end do
          ! Create sine and cosine Fourier series components
        hA = LAM_HR64P*this%m_A
        API = this%m_A/LAM_PI
        insamp = 1._R64P/DBLE(this%m_nsamp)
        do k = 1, this%m_maxk
            do j = 1, this%m_nsamp
                  this%m_sinit = this%m_sinit+this%m_stsinc
                  delta = DBLE(j)*insamp
                  t = this%m_sinit*delta

                  this%m_scomp(k,j) = -1**k*DSIN(LAM_2PI*k*this%m_freq*t)/k
                  this%m_ccomp(k,j) = -1**k*DCOS(LAM_2PI*k*this%m_freq*t)/k
             end do
        end do
          ! Do summation over the rows
        this%m_sawtooth = SUM(this%m_scomp,dim=2)
        tcos = SUM(this%m_ccomp,dim=2)
          ! Create sawtooth signal (final approximation) and
          ! create signal complex representation
        do j = 1, this%m_nsamp
              this%m_sinit = this%m_sinit+this%m_stsinc
              delta = DBLE(j)*insamp
              t = this%m_sinit*delta
              this%m_wct(j) = t
              this%m_sawtooth(j) = hA-(API*this%m_sawtooth(j))
              tcos(j) = hA-(API*tcos(j))
              this%m_cform(i) = DCMPLX(tcos(j),this%m_sawtooth(j))
        end do
          ! Compute natural envelope
        call vcmag(this%m_cform,this%m_nenvp)
          ! Create electrical field far from the emitter.
        do k = 1, this%m_maxk
            do j = 1, this%m_nsamp
                  this%m_sinit = this%m_sinit+this%m_stsinc
                  delta = DBLE(j)*insamp
                  t = this%m_sinit*delta
                  t2 = t-r(j)/LAM_c
                  this%m_scomp(k,j) = -1**k*DSIN(LAM_2PI*k*this%m_freq*t2)/k
                  this%m_ccomp(k,j) = -1**k*DCOS(LAM_2PI*k*this%m_freq*t2)/k
            end do
        end do
          ! Do summation over the rows
        tc = SUM(this%m_ccomp,dim=2)
        ts = SUM(this%m_scomp,dim=2)
        do j = 1, this%m_nsamp
               tc(j) = hA-(API*tc(j))
               ts(j) = hA-(API*ts(j))
               this%m_E(i) = DCMPLX(tc(j),ts(j))
        end do
          ! Compute A(phi,theta)/r - magnitude of electric field E
        call vcmag(this%m_E,this%m_amp)
          ! Compute time-average power density i.e. 0.5xExE*/n
        do j = 1, this%m_nsamp
              this%m_S(j) = LAM_HR64P*this%m_E(j)*DCONJG(this%m_E(j))*0.0026525198938992_R64P
        end do

        if(profiling) then
                if(ifail.EQ.0) then
                    call qpctimer_stop(qpctimer,ifail)
                    call qpctimer_delta(qpctimer,bfail)
                    if(bfail .EQ. .false.) then
                         call qpctimer_print(qpctimer)
                    else
                         write(stderr,*) "mod_sawtooth_signal/sawtooth_signal: qpctimer_delta: failed to compute delta measurement!!"
                    end if
                else
                    write(stderr,*) "mod_sawtooth_signal/sawtooth_signal: Unable to read performance counter -- fatal!!"
                end if
        end if
    end subroutine sawtooth_signal

    !======================================================60
    !  @subroutine: phase_noise_sawtooth_signal
    !              Physical representation of Sawtooth wave
    !              approximated by K sinusoids (Fourier-
    !              series)
    !              This signal is corrupted by phase additive
    !              noise.
    !======================================================60
    subroutine phase_noise_sawtooth_signal(this,ierr,dur,A,                 &
                                  sinit,stsinc,freq,phnoise,r,h,v,          &
                                  verbose,logging,filename,append,          &
                                  profiling,qpctimer  )
          use mod_print_error,  only : handle_fatal_memory_error
          use mod_constants,    only : LAM_ZR8, LAM_HR64P, LAM_2PI, LAM_PI, LAM_PINF
          class(SawtoothSignal_t),     intent(inout) :: this
          integer(I32P),               intent(inout) :: ierr
          real(R64P),                  intent(inout) :: dur,A
          real(R64P),                  intent(in)    :: sinit,stsinc,freq
          real(R64P), dimension(:),    intent(in)    :: phnoise,r
!DIR$     ASSUME_ALIGNED phnoise:64,r:64
          complex(R64P), dimension(:), intent(in)    :: h,v
!DIR$     ASSUME_ALIGNED h:64,v:64
          logical(I32P),               intent(in)    :: verbose,logging
          character(len=*),            intent(in)    :: filename
          logical(I32P),               intent(in)    :: append,profiling
          type(QPCTimer_t),            intent(inout) :: qpctimer
          ! Locals

          character(len=256) :: emsg
          integer(I32P)      :: k,j,aerr
          integer(BOOL)      :: ifail
          logical(I32P)      :: bfail
          real(R64P)         :: insamp,t,t2,delta,hA,API,tn
          real(R64P),    allocatable, dimension(:) :: tcos,ts,tc
!DIR$     ATTRIBUTES ALIGN : 64 :: tcos
!DIR$     ATTRIBUTES ALIGN : 64 :: ts
!DIR$     ATTRIBUTES ALIGN : 64 :: tc
          ! Start of executable statements

          if(ierr < 0_I32P) ierr = 0_I32P
          if(dur <= LAM_ZR8 .OR. &
             A   <= LAM_ZR8  ) then
              ierr = -1_I32P
              return
          end if
          this%m_dur    = dur
          this%m_A      = A
          this%m_sinit  = sinit
          this%m_stsinc = stsinc
          this%m_freq   = freq
          associate(n=>this%m_nsamp)
              allocate(  tcos(n),             &
                         ts(n),               &
                         tc(n),               &
                         STAT=aerr,           &
                         ERRMSG=emsg      )
          end associate
          if(aerr /= 0) then

              call handle_fatal_memory_error( logging,verbose,append,filename,   &
                                              "logger:541 --> mod_sawtooth_singnal/phase_noise_sawtooth_signal: Memory Allocation Failure!!" , &
                                              "mod_sawtooth_signal/phase_noise_sawtooth_signal:541 -- Memory Allocation Failure!" ,     &
                                              emsg,__LINE__ )
          end if
          tcos = LAM_PINF
          ts   = LAM_PINF
          tc   = LAM_PINF
            if(profiling) then
              call qpctimer_start(qpctimer,ifail)
              if(ifail == 0) then
                  write(stderr,*) "mod_sawtooth_signal/phase_noise_signal: qpctimer_start failed to query performance frequency counter!"
              end if
          end if
          ! Set up jones vector field
          do j = 1, this%m_nsamp
              this%m_jvec(j) = JonesVector_t(h(j),v(j))
          end do
           ! Create sine and cosine Fourier series components
          hA = LAM_HR64P*this%m_A
          API = this%m_A/LAM_PI
          insamp = 1._R64P/DBLE(this%m_nsamp)
          do k = 1, this%m_maxk
              do j = 1, this%m_nsamp
                  this%m_sinit = this%m_sinit+this%m_stsinc
                  delta = DBLE(j)*insamp
                  t = this%m_sinit*delta
                  tn = t+phnoise(j)
                  this%m_scomp(k,j) = -1**k*DSIN(LAM_2PI*k*this%m_freq*tn)/k
                  this%m_ccomp(k,j) = -1**k*DCOS(LAM_2PI*k*this%m_freq*tn)/k
              end do
          end do
          ! Do summation over the rows
          this%m_sawtooth = SUM(this%m_scomp,dim=2)
          tcos = SUM(this%m_ccomp,dim=2)
          ! Create sawtooth signal (final approximation) and
          ! create signal complex representation
          do j = 1, this%m_nsamp
              this%m_sinit = this%m_sinit+this%m_stsinc
              delta = DBLE(j)*insamp
              t = this%m_sinit*delta
              tn = t+phnoise(j)
              this%m_wct(j) = tn
              this%m_sawtooth(j) = hA-(API*this%m_sawtooth(j))
              tcos(j) = hA-(API*tcos(j))
              this%m_cform(i) = DCMPLX(tcos(j),this%m_sawtooth(j))
          end do
          ! Compute natural envelope
          call vcmag(this%m_cform,this%m_nenvp)
          ! Create electrical field far from the emitter.
          do k = 1, this%m_maxk
              do j = 1, this%m_nsamp
                  this%m_sinit = this%m_sinit+this%m_stsinc
                  delta = DBLE(j)*insamp
                  t = this%m_sinit*delta
                  t2 = t-r(j)/LAM_c
                  tn = t2+phnoise(j)
                  this%m_scomp(k,j) = -1**k*DSIN(LAM_2PI*k*this%m_freq*tn)/k
                  this%m_ccomp(k,j) = -1**k*DCOS(LAM_2PI*k*this%m_freq*tn)/k
              end do
          end do
          ! Do summation over the rows
          tc = SUM(this%m_ccomp,dim=2)
          ts = SUM(this%m_scomp,dim=2)
          do j = 1, this%m_nsamp
               tc(j) = hA-(API*tc(j))
               ts(j) = hA-(API*ts(j))
               this%m_E(i) = DCMPLX(tc(j),ts(j))
          end do
          ! Compute A(phi,theta)/r - magnitude of electric field E
          call vcmag(this%m_E,this%m_amp)
          ! Compute time-average power density i.e. 0.5xExE*/n
          do j = 1, this%m_nsamp
              this%m_S(j) = LAM_HR64P*this%m_E(j)*DCONJG(this%m_E(j))*0.0026525198938992_R64P
          end do

          if(profiling) then
                if(ifail == 0) then
                    call qpctimer_stop(qpctimer,ifail)
                    call qpctimer_delta(qpctimer,bfail)
                    if(bfail == .false.) then
                         call qpctimer_print(qpctimer)
                    else
                         write(stderr,*) "mod_sawtooth_signal/phase_noise_sawtooth_signal: qpctimer_delta: failed to compute delta measurement!!"
                    end if
                else
                    write(stderr,*) "mod_sawtooth_signal/phase_sawtooth_signal_signal: Unable to read performance counter -- fatal!!"
                end if
          end if
   end subroutine phase_noise_sawtooth_signal

    !======================================================60
    !  subroutine: additive_noise_signal
    !              Physical representation of Sawtooth wave
    !              approximated by K sinusoids (Fourier-
    !              series)
    !              This signal is corrupted by phase additive
    !              noise and by background additive noise.
    !======================================================60
    subroutine additive_noise_sawtooth_signal(this,ierr,dur,A,             &
                                  sinit,stsinc,freq,phnoise,bnoise,r,h,v,  &
                                  verbose,logging,filename,append,         &
                                  profiling,qpctimer  )
          use mod_print_error,  only : handle_fatal_memory_error
          use mod_constants,    only : LAM_ZR8, LAM_HR64P, LAM_2PI, LAM_PI
          class(SawtoothSignal_t),     intent(inout) :: this
          integer(I32P),               intent(inout) :: ierr
          real(R64P),                  intent(inout) :: dur,A
          real(R64P),                  intent(in)    :: sinit,stsinc,freq
          real(R64P), dimension(:),    intent(in)    :: phnoise,bnoise,r
!DIR$     ASSUME_ALIGNED phnoise:64,bnoise:64,r:64
          complex(R64P), dimension(:), intent(in)    :: h,v
!DIR$     ASSUME_ALIGNED h:64,v:64
          logical(I32P),               intent(in)    :: verbose,logging
          character(len=*),            intent(in)    :: filename
          logical(I32P),               intent(in)    :: append,profiling
          type(QPCTimer_t),            intent(inout) :: qpctimer
          ! Locals

          character(len=256) :: emsg
          integer(I32P)      :: k,j,aerr
          integer(BOOL)      :: ifail
          logical(I32P)      :: bfail
          real(R64P)         :: insamp,t,t2,delta,hA,API,tn
          real(R64P),    allocatable, dimension(:) :: tcos,ts,tc
!DIR$     ATTRIBUTES ALIGN : 64 :: tcos
!DIR$     ATTRIBUTES ALIGN : 64 :: ts
!DIR$     ATTRIBUTES ALIGN : 64 :: tc
          ! Start of executable statements
          if(ierr < 0_I32P) ierr = 0_I32P
          if(dur <= LAM_ZR8 .OR. &
             A   <= LAM_ZR8 ) then
             ierr = -1_I32P
              return
          end if
          this%m_dur = dur
          this%m_A  = A
          this%m_sinit = sinit
          this%m_stsinc = stsinc
          this%m_freq = freq
          associate(n=>this%m_nsamp)
              allocate(  tcos(n),             &
                         ts(n),               &
                         tc(n),               &
                         STAT=aerr,           &
                         ERRMSG=emsg   )
          end associate
          if(aerr /= 0) then

              call handle_fatal_memory_error(logging,verbose,append,filename,    &
                                             "logger:697 --> mod_sawtooth_singnal/additive_noise_sawtooth_signal: Memory Allocation Failure!!" , &
                                             "mod_sawtooth_signal/additive_noise_sawtooth_signal:697 -- Memory Allocation Failure!" , &
                                             emsg,__LINE__ )
          end if
            if(profiling) then
              call qpctimer_start(qpctimer,ifail)
              if(ifail == 0) then
                  write(stderr,*) "mod_sawtooth_signal/additive_noise_sawtooth_signal: qpctimer_start failed to query performance frequency counter!"
              end if
          end if
          ! Set up jones vector field
          do j = 1, this%m_nsamp
              this%m_jvec(j) = JonesVector_t(h(j),v(j))
          end do
           ! Create sine and cosine Fourier series components
          hA = LAM_HR64P*this%m_A
          API = this%m_A/LAM_PI
          insamp = 1._R64P/DBLE(this%m_nsamp)
          do k = 1, this%m_maxk
              do j = 1, this%m_nsamp
                  this%m_sinit = this%m_sinit+this%m_stsinc
                  delta = DBLE(j)*insamp
                  t = this%m_sinit*delta
                  tn = t+phnoise(j)
                  this%m_scomp(k,j) = -1**k*DSIN(LAM_2PI*k*this%m_freq*tn)/k
                  this%m_scomp(k,j) = bnoise(j)+this%m_scomp(k,j)
                  this%m_ccomp(k,j) = -1**k*DCOS(LAM_2PI*k*this%m_freq*tn)/k
                  this%m_ccomp(k,j) = bnoise(j)+this%m_ccomp(k,j)
              end do
          end do
          ! Do summation over the rows
          this%m_sawtooth = SUM(this%m_scomp,dim=2)
          tcos = SUM(this%m_ccomp,dim=2)
          ! Create sawtooth signal (final approximation) and
          ! create signal complex representation
          do j = 1, this%m_nsamp
              this%m_sinit = this%m_sinit+this%m_stsinc
              delta = DBLE(j)*insamp
              t = this%m_sinit*delta
              tn = t+phnoise(j)
              this%m_wct(j) = tn
              this%m_sawtooth(j) = hA-(API*this%m_sawtooth(j))
              tcos(j) = hA-(API*tcos(j))
              this%m_cform(i) = DCMPLX(tcos(j),this%m_sawtooth(j))
          end do
          ! Compute natural envelope
          call vcmag(this%m_cform,this%m_nenvp)
          ! Create electrical field far from the emitter.
          do k = 1, this%m_maxk
              do j = 1, this%m_nsamp
                  this%m_sinit = this%m_sinit+this%m_stsinc
                  delta = DBLE(j)*insamp
                  t = this%m_sinit*delta
                  t2 = t-r(j)/LAM_c
                  tn = t2+phnoise(j)
                  this%m_scomp(k,j) = -1**k*DSIN(LAM_2PI*k*this%m_freq*tn)/k
                  this%m_scomp(k,j) = bnoise(j)+this%m_scomp(k,j)
                  this%m_ccomp(k,j) = -1**k*DCOS(LAM_2PI*k*this%m_freq*tn)/k
                  this%m_ccomp(k,j) = bnoise(j)+this%m_ccomp(k,j)
              end do
          end do
          ! Do summation over the rows
          tc = SUM(this%m_ccomp,dim=2)
          ts = SUM(this%m_scomp,dim=2)
          do j = 1, this%m_nsamp
               tc(j) = hA-(API*tc(j))
               ts(j) = hA-(API*ts(j))
               this%m_E(i) = DCMPLX(tc(j),ts(j))
          end do
          ! Compute A(phi,theta)/r - magnitude of electric field E
          call vcmag(this%m_E,this%m_amp)
          ! Compute time-average power density i.e. 0.5xExE*/n
          do j = 1, this%m_nsamp
              this%m_S(j) = LAM_HR64P*this%m_E(j)*DCONJG(this%m_E(j))*0.0026525198938992_R64P
          end do

          if(profiling) then
                if(ifail.EQ.0) then
                    call qpctimer_stop(qpctimer,ifail)
                    call qpctimer_delta(qpctimer,bfail)
                    if(bfail .EQ. .false.) then
                         call qpctimer_print(qpctimer)
                    else
                         write(stderr,*) "mod_sawtooth_signal/additive_noise_sawtooth_signal: qpctimer_delta: failed to compute delta measurement!!"
                    end if
                else
                    write(stderr,*) "mod_sawtooth_signal/additive_noise_sawtooth_signal: Unable to read performance counter -- fatal!!"
                end if
          end if
   end subroutine additive_noise_sawtooth_signal

    !======================================================60
    ! Getter pure functions
    !======================================================60

    pure function get_name(this) result(name)
          implicit none
          class(SawtoothSignal_t), intent(in) :: this
          ! Locals
          character(len=64) :: name
          ! Start of executable statements
          name = this%m_name
    end function

    pure function get_sid(this) result(sid)
          implicit none
          class(SawtoothSignal_t), intent(in) :: this
          ! Locals
          integer(I32P) :: sid
          ! Start of executable statements
          sid = this%m_sid
    end function

    pure function get_nsamp(this) result(nsamp)
          implicit none
          class(SawtoothSignal_t), intent(in) :: this
          ! Locals
          integer(I32P) :: nsamp
          ! Start of executable statements
          nsamp = this%m_nsamp
    end function

    pure function get_maxk(this) result(maxk)
          implicit none
          class(SawtoothSignal_t), intent(in) :: this
          ! Locals
          integer(I32P) :: maxk
          ! Start of executable statements
          maxk = this%m_maxk
    end function

    pure function get_dur(this) result(dur)
          implicit none
          class(SawtoothSignal_t), intent(in) :: this
          ! Locals
          real(R64P) :: dur
          ! Start of executable statements
          dur = this%m_dur
    end function

    pure function get_A(this) result(A)
          implicit none
          class(SawtoothSignal_t), intent(in) :: this
          ! Locals
          real(R64P) :: A
          ! Start of executable statements
          A = this%m_A
    end function

    pure function get_sinit(this) result(sinit)
          implicit none
          class(SawtoothSignal_t), intent(in) :: this
          ! Locals
          real(R64P) :: sinit
          ! Start of executable statements
          sinit = this%m_sinit
    end function

    pure function get_stsinc(this) result(stsinc)
          implicit none
          class(SawtoothSignal_t), intent(in) :: this
          ! Locals
          real(R64P) :: stsinc
          ! Start of executable statemetns
          stsinc = this%m_stsinc
    end function

    pure function get_freq(this) result(freq)
          implicit none
          class(SawtoothSignal_t), intent(in) :: this
          ! Locals
          real(R64P) :: freq
          ! Start of executable statements
          freq = this%m_freq
    end function

    !======================================================60
    !    Computational procedures
    !======================================================60
    ! TODO:
    !        ! Consider using CADNA to test for cancellation errors
    subroutine dphi_dt(this,dphi,sfac)
          use mod_constants, only : LAM_IZER8, LAM_MEPS8
          class(SawtoothSignal_t),  intent(in)     :: this
          real(R64P), dimension(:), intent(out)    :: dphi
!DIR$     ASSUME_ALIGNED dphi:64
          integer(I64P),            intent(inout)  :: sfac
          ! Locals
          integer(I32P) :: i
          real(R64P)    :: eps,isfac,tmp
          ! Start of executable sttements
          if(sfac <=  LAM_IZER8) then
              sfac =  1000000000000000_I64P
          end if
          isfac = 1._R64P/DBLE(sfac)
          eps = LAM_MEPS8**0.3333333333333333333333333333_R64P
          do i = 2, this%m_nsamp-1
              tmp = this%m_wct(i+1)-this%m_wct(i-1)
              dphi(i) = this%m_wct(i+1)-this%m_wct(i-1) / &
                        (2._R64P*eps*DMAX1(DABS(tmp),isfac)*DSIGN(tmp,tmp))
          end do
    end subroutine  dphi_dt

     !============================================
    !   Read/write procedures
    !============================================
       subroutine read_state(this,form,unit,ioerr)

          class(SawtoothSignal_t), intent(in) :: this
          character(len=*),        intent(in) :: form
          integer(I32P),           intent(in) :: unit
          integer(I32P),           intent(inout) :: ioerr
          ! Start of executable statements
          select case(adjustl(trim(form)))
          case ("*")
              READ(unit,*,iostat=ioerr) this
          case default
              READ(unit,adjustl(trim(form)),iostat=ioerr) this
          end select
    end subroutine read_state

    subroutine write_state(this,form,unit,ioerr)

          class(SawtoothSignal_t), intent(in) :: this
          character(len=*),        intent(in) :: form
          integer(I32P),           intent(in) :: unit
          integer(I32P),           intent(inout) :: ioerr
          ! Start of executable statements
          select case(adjustl(trim(form)))
          case ("*")
              WRITE(unit,*,iostat=ioerr) this
          case default
              WRITE(unit,adjustl(trim(form)),iostat=ioerr) this
          end select
    end subroutine write_state

    !======================================================60
    !  subroutine: dbg_info
    !======================================================60
    subroutine dbg_info(this,verbose)

          class(SawtoothSignal_t), intent(in) :: this
          logical(I32P),           intent(in) :: verbose
          ! Locals
          ! Start of executable statements
          call DATE_AND_TIME(date=dstr,time=tstr)
          print*, "======================================================"
          print*, "   ***Printing components of SawtoothSignal_t***"
          print*, "   printing subroutine called at: " , &
                     sdate(1:4),"-",sdate(5:6),"-",sdate(7:8), " ", &
                     stime(1:2),":",stime(3:4),":",stime(5:6)
          print*, "           Dumping scalar components "
          print*, "======================================================"
          print*, "Name:               ", this%m_name
          print*, "Signal ID:          ", this%m_sid
          print*, "Number of samples:  ", this%m_nsamp
          print*, "Maximum 'k':        ", this%m_maxk
          print*, "Signal duration:    ", this%m_dur
          print*, "scalar amplitude:   ", this%m_A
          print*, "Initial time-point: ", this%m_sinit
          print*, "Time-step increment:", this%m_stsinc
          print*, "Carrier frequency:  ", this%m_freq
          print*, "           Dumping array components  "
          print*, "======================================================"

          print*, "Signal polarization:", this%m_jvec
          if(verbose) then
              print*, "Sine series components:  ", this%m_scomp
              print*, "Cosine series components:", this%m_ccomp
          end if
          print*, "Time (t):             ",   this%m_wct
          print*, "Natural Envelope:     ",   this%m_nenvp
          print*, "Sawtooth time-domain: ",   this%m_sawtooth
          print*, "Complex form:         ",   this%m_cform
          print*, "Electric field:       ",   this%m_E
          print*, "Amplitude:            ",   this%m_amp
          print*, "Time-average power:   ",   this%m_S
    end subroutine dbg_info

    !============================================
    !  Operator assignment (=)
    !============================================
    subroutine copy_assign(lhs,rhs)
          class(SawtoothSignal_t),  intent(inout) :: lhs
          class(SawtoothSignal_t),  intent(in)    :: rhs
          ! Start of executable ststements
          lhs%m_name     = rhs%m_name
          lhs%m_sid      = rhs%m_sid
          lhs%m_nsamp    = rhs%m_nsamp
          lhs%m_maxk     = rhs%m_maxk
          lhs%m_dur      = rhs%m_dur
          lhs%m_A        = rhs%m_A
          lhs%m_sinit    = rhs%m_sinit
          lhs%m_stsinc   = rhs%m_stsinc
          lhs%m_freq     = rhs%m_freq
          lhs%m_jvec     = rhs%m_jvec
          lhs%m_scomp    = rhs%m_scomp
          lhs%m_ccomp    = rhs%m_ccomp
          lhs%m_wct      = rhs%m_wct
          lhs%m_nenvp    = rhs%m_nenvp
          lhs%m_sawtooth = rhs%m_sawtooth
          lhs%m_cform    = rhs%m_cform
          lhs%m_E        = rhs%m_E
          lhs%m_amp      = rhs%m_amp
          lhs%m_S        = rhs%m_S
    end subroutine copy_assign

end module mod_sawtooth_signal


module utils

implicit none

integer, parameter :: rk = selected_real_kind(15,300)

contains

subroutine printMatrix(A, name)

    real(rk), intent(in) :: A(:,:)
    character(*), intent(in), optional :: name
    integer :: rows, cols, slen
    integer :: i, j
    character(255) :: fmt
    character(255) :: sfmt

    rows = size(A,1)
    cols = size(A,2)
    slen = len_trim(name)

    if (slen<10) then
        write(sfmt, '(A,I1,A)') '(TL1,A,A',slen,',A,I3,A,I3,A)'
    else if (slen<100) then
        write(sfmt, '(A,I2,A)') '(TL1,A,A',slen,',A,I3,A,I3,A)'
    else
        write(sfmt, '(A,I3,A)') '(TL1,A,A',slen,',A,I3,A,I3,A)'
    end if

    if (cols<10) then
        write(fmt, '(A,I1,A)') '(', cols, 'G10.3)'
    else if (cols<100) then
        write(fmt, '(A,I2,A)') '(', cols, 'G10.3)'
    else
        write(fmt, '(A,I3,A)') '(', cols, 'G10.3)'
    end if

    write(*,'(TL1,A)') '-------------------------------------'
    if (present(name)) then
        write(*,sfmt) 'Matrix ',trim(name),' (',rows, ' x ', cols, ')'
    else
        write(*,'(TL1,A,I3,A,I3,A)') 'Matrix (',rows, ' x ', cols, ')'
    end if
    write(*,'(TL1,A)') '-------------------------------------'

    do i=1,rows
        write(*,fmt) (A(i,j), j=1,cols)
    end do

    write(*,'(TL1,A)') '-------------------------------------'

end subroutine printMatrix

subroutine printVector(A, name)

    real(rk), intent(in) :: a(:)
    character(*), intent(in), optional :: name
    integer :: cols, slen
    integer :: i
    character(255) :: fmt
    character(255) :: sfmt

    cols = size(A,1)
    slen = len_trim(name)

    if (slen<10) then
        write(sfmt, '(A,I1,A)') '(TL1,A,A',slen,',A,I3,A)'
    else if (slen<100) then
        write(sfmt, '(A,I2,A)') '(TL1,A,A',slen,',A,I3,A)'
    else
        write(sfmt, '(A,I3,A)') '(TL1,A,A',slen,',A,I3,A)'
    end if

    if (cols<10) then
        write(fmt, '(A,I1,A)') '(', cols, 'G10.3)'
    else if (cols<100) then
        write(fmt, '(A,I2,A)') '(', cols, 'G10.3)'
    else
        write(fmt, '(A,I3,A)') '(', cols, 'G10.3)'
    end if

    write(*,'(TL1,A)') '-------------------------------------'
    if (present(name)) then
        write(*,sfmt) 'Vector ',trim(name),' (',cols,')'
    else
        write(*,'(TL1,A,I3,A)') 'Vector (',cols,')'
    end if
    write(*,'(TL1,A)') '-------------------------------------'

    write(*,fmt) (a(i), i=1,cols)

    write(*,'(TL1,A)') '-------------------------------------'

end subroutine printVector

subroutine randMat(A, randMin, randMax)

    real(rk), intent(inout) :: A(:,:)
    real(rk), intent(in) :: randMin, randMax

    call random_number(A)

    !    min ---- max

    A = randMin + (randMax-randMin) * A

end subroutine randMat

subroutine randVal(a, randMin, randMax)

    real(rk), intent(inout) :: a
    real(rk), intent(in) :: randMin, randMax

    call random_number(a)
    a = randMin + (randMax-randMin) * a

end subroutine randVal

subroutine initRand()
    integer :: values(1:8), k
    integer, dimension(:), allocatable :: seed
    real(8) :: r
    call date_and_time(values=values)
    call random_seed(size=k)
    allocate(seed(1:k))
    seed(:) = values(8)
    call random_seed(put=seed)
end subroutine

end module utils


!----------------------------------------------------------------------
!----------------------------------------------------------------------
!   ivp :    Time integration (using Implicit Euler)
!----------------------------------------------------------------------
!----------------------------------------------------------------------

      SUBROUTINE FUNC(NDIM,U,ICP,PAR,IJAC,F,DFDU,DFDP)
!     ---------- ----

      IMPLICIT NONE
      INTEGER, INTENT(IN) :: NDIM, ICP(*), IJAC
      DOUBLE PRECISION, INTENT(IN) :: U(NDIM), PAR(*)
      DOUBLE PRECISION, INTENT(OUT) :: F(NDIM)
      DOUBLE PRECISION, INTENT(INOUT) :: DFDU(NDIM,NDIM), DFDP(NDIM,*)

      DOUBLE PRECISION E

       E=EXP(-PAR(3)*U(1))
       F(1)=PAR(2)*U(1)*(1-U(1)) - U(1)*U(2) - PAR(1)*(1-E)
       F(2)=-U(2) + PAR(4)*U(1)*U(2)

      END SUBROUTINE FUNC

      SUBROUTINE STPNT(NDIM,U,PAR,T)
!     ---------- -----

      IMPLICIT NONE
      INTEGER, INTENT(IN) :: NDIM
      DOUBLE PRECISION, INTENT(INOUT) :: U(NDIM),PAR(*)
      DOUBLE PRECISION, INTENT(IN) :: T

!      **Set (constant) parameters
       PAR(1)=0.
       PAR(2)=3.0
       PAR(3)=5.0
       PAR(4)=3.0

!      **Set initial values
       U(1)=0.3
       U(2)=0.3

      END SUBROUTINE STPNT

      SUBROUTINE BCND
      END SUBROUTINE BCND

      SUBROUTINE ICND
      END SUBROUTINE ICND

      SUBROUTINE FOPT
      END SUBROUTINE FOPT

      SUBROUTINE PVLS
      END SUBROUTINE PVLS


subroutine channel()
    use settings
    use partitioner
	use io
	use schemes
	use solver
	use boundaryConditions
	use IBM

	implicit none

    !double precision ,dimension(Np,3) :: Yp, Vp, Vp0		! Particle variables

	double precision ,dimension(ny-2,1:m) :: D_CONV_U, D_CONV_V, DEL2U, DEL2V, DpDx, DpDy

    double precision :: R, E_P, E_U
	integer :: NITS_final_P, NITS_final_U

	integer :: i, j

    integer :: id, Nproc, ierr

    ! for io
    double precision :: Umax, Pmax, Umax_loc, Pmax_loc

    call mpi_comm_rank(MPI_COMM_WORLD, id, ierr)
    call mpi_comm_size(MPI_COMM_WORLD, Nproc, ierr)

	! Initialize Flow Variables
	field%u=0; field%v=0; field%P=0
	!field%u_last=0; field%v_last=0;
	field%u_star=0; field%v_star=0
	field%u_star2=0; field%v_star2=0

    !if(id==0) print*, 'flow initialized.'

	! Pressure driven force
	forces%F = 8*nu*U0/Ly**2
    !forces%F = 0

	forces%Hx=0; forces%Hy=0
	forces%Bx=0; forces%By=0;

	! construct x,y,z matrices (mesh)
	do i=1,m; mesh%x(:,i) = (iStartx+i-1)*dx;	end do
	do i=1,ny; mesh%y(i,:) = (i-1)*dy;	end do

	! Velocity IC
	call setIC(field%u,field%v,field%P,mesh%x,mesh%y)

	![u,v] = setVelocityBC1(u,v,dx,dy,dt,nu);

	! write data before starting simulation
	call writeToTecplot2D_MPI(mesh%x,mesh%y,field%u,field%v,field%P,0)

	do i=1,nt

		!call CONV_U_UPWIND(D_CONV_U,field%u,field%v)
		!call CONV_V_UPWIND(D_CONV_V,field%u,field%v)

		!call DEL2(DEL2U,field%u)
		!call DEL2(DEL2V,field%v)

		!field%u_star(2:ny-1,1:m) = field%u(2:ny-1,1:m) + dt*(-D_CONV_U + nu*DEL2U)
		!field%v_star(2:ny-1,1:m) = field%v(2:ny-1,1:m) + dt*(-D_CONV_V + nu*DEL2V)

		! call setVelocityBC_MPI(u,v,w)

		!call calcUstar_AB(field%u_star,field%v_star,field%u,field%v,field%u_last,field%v_last)

		! Total body force
		forces%Bx = forces%F + forces%Hx
		forces%By = forces%Hy

		call calcUstar_Implicit(field%u_star,field%v_star,E_U,NITS_final_U,field%u,field%v,forces%Bx,forces%By)

        !field%u_last = field%u
        !field%v_last = field%v

        ! COMMUNICATE (u_star)
        !call communicate(field%u_star)
        !call communicate(field%v_star)

        !if(id==0) print*, 'communication done'

        !call setVelocityBC_MPI(field%u_star,field%v_star)

		call PressureSolver_MPI(field%P,E_P,NITS_final_P,field%u_star,field%v_star)
		!call PressureSolver_MPI_BiCGStab(field%P,E_P,NITS_final_P,field%u_star,field%v_star)

        !if(id==0) print*, 'Pressure solver done: ', NITS_final
		call GRAD(DpDx,DpDy,field%P)

        !if(id==0) print*, 'GRAD calculated'

		field%u_star2(2:ny-1,1:m) = field%u_star(2:ny-1,1:m) - dt/rho*DpDx
		field%v_star2(2:ny-1,1:m) = field%v_star(2:ny-1,1:m) - dt/rho*DpDy

        call setVelocityBC_MPI(field%u_star2,field%v_star2)

		call communicate(field%u_star2)
		call communicate(field%v_star2)

        !if(id==0) print*, 'u_star2 calculated'

		! call setVelocityBC2(u_star2,v_star2,w_star2,u_star,v_star,w_star,P)

		! Calculate force for IBM
		!call IBMForceCircle(forces%Hx,forces%Hy,field%u,field%v,mesh%x,mesh%y)

		! march in time using force
		! (Entire x dimension is used, since periodic BC in that direction)
		field%u = field%u_star2
		field%v = field%v_star2

        !call setVelocityBC_MPI(field%u,field%v)

        ! COMMUNICATE (u)
        !call communicate(field%u)
        !call communicate(field%v)

        !if(id==0) print*, 'field updated'

        Umax_loc = maxval(field%u(:,1:m))
        Pmax_loc = maxval(field%P(:,1:m))
        call mpi_reduce(Umax_loc,Umax,1,MPI_DOUBLE_PRECISION,MPI_MAX,0,MPI_COMM_WORLD,ierr)
        call mpi_reduce(Pmax_loc,Pmax,1,MPI_DOUBLE_PRECISION,MPI_MAX,0,MPI_COMM_WORLD,ierr)
        !if(id==0) print*, 'reduction for io done'

        if (id==0) then
            if (mod(i,nLog)==0) then
                call writeLogToTerminal_MPI(i, &
                'Umax','double',Umax, &
                'Pmax','double',Pmax, &
                'Pressure Error','double',E_P, &
                'Pressure Iterations','integer',dble(NITS_final_P), &
				'Velocity Error','double',E_U, &
				'Velocity Iterations','integer',dble(NITS_final_U) )
            end if

        end if

        if (mod(i,nWrite)==0) then
            call writeToTecplot2D_MPI(mesh%x,mesh%y,field%u,field%v,field%P,i)
        end if

        call mpi_barrier(MPI_COMM_WORLD,ierr)

	end do

end subroutine channel


cdis
cdis    Open Source License/Disclaimer, Forecast Systems Laboratory
cdis    NOAA/OAR/FSL, 325 Broadway Boulder, CO 80305
cdis
cdis    This software is distributed under the Open Source Definition,
cdis    which may be found at http://www.opensource.org/osd.html.
cdis
cdis    In particular, redistribution and use in source and binary forms,
cdis    with or without modification, are permitted provided that the
cdis    following conditions are met:
cdis
cdis    - Redistributions of source code must retain this notice, this
cdis    list of conditions and the following disclaimer.
cdis
cdis    - Redistributions in binary form must provide access to this
cdis    notice, this list of conditions and the following disclaimer, and
cdis    the underlying source code.
cdis
cdis    - All modifications to this software must be clearly documented,
cdis    and are solely the responsibility of the agent making the
cdis    modifications.
cdis
cdis    - If significant modifications or enhancements are made to this
cdis    software, the FSL Software Policy Manager
cdis    (softwaremgr@fsl.noaa.gov) should be notified.
cdis
cdis    THIS SOFTWARE AND ITS DOCUMENTATION ARE IN THE PUBLIC DOMAIN
cdis    AND ARE FURNISHED "AS IS."  THE AUTHORS, THE UNITED STATES
cdis    GOVERNMENT, ITS INSTRUMENTALITIES, OFFICERS, EMPLOYEES, AND
cdis    AGENTS MAKE NO WARRANTY, EXPRESS OR IMPLIED, AS TO THE USEFULNESS
cdis    OF THE SOFTWARE AND DOCUMENTATION FOR ANY PURPOSE.  THEY ASSUME
cdis    NO RESPONSIBILITY (1) FOR THE USE OF THE SOFTWARE AND
cdis    DOCUMENTATION; OR (2) TO PROVIDE TECHNICAL SUPPORT TO USERS.
cdis
cdis
cdis
cdis
cdis
      program test
      implicit none
      real elev,slantkm,slant_range
      real height,range
      real height_grid,r_range
      real rheight_radar
c
c     Doviak Zrnic stuff
c
      real re,forthre,fthrsqd
      parameter (re=6371 000.,
     :           forthre=(4.*re/3.),
     :           fthrsqd=(forthre*forthre))
c
c     Albers' stuff
c
      real rpd,mpd,radius_earth,radius_earth_8_thirds
      real hor_dist,curvature,height_factor
  10  print *, '  Enter elev (deg), slant range (km): '
      read(5,*) elev,slantkm
      if(slantkm.eq.0.) STOP
      slant_range=1000.*slantkm
      rheight_radar=0.
c
c     Doviak Zrnic Calculation
c
      height=sqrt(slant_range*slant_range + fthrsqd +
     :           2.*forthre*slant_range*sind(elev)) -
     :           forthre
c
      range=forthre*asin((slant_range*cosd(elev)) /
     :                   (forthre + height))
      range=0.001*range
c
      height=height+rheight_radar
c
      print *, ' Height, Range (D & Z ): ',height,range
c
c     Albers' Calculation
c
       rpd = 3.141592653589/180.
       mpd = 111194.
       radius_earth = 6371.e3
       radius_earth_8_thirds = 6371.e3 * 2.6666666

       hor_dist = slant_range * cosd(elev)

       curvature = hor_dist **2 / radius_earth_8_thirds
       height_grid =
     1  slant_range * sind(elev) + curvature + rheight_radar

       height_factor =
     1   (radius_earth + 0.5 * (rheight_radar + height_grid))
     1  /radius_earth

      r_range = hor_dist / height_factor * .001

      print *, ' Height, Range (Albers): ',height_grid,r_range

      GO TO 10
      END


c-----------------------------------------------------------------------
c
c     Lam-Bremhorst Kay-Epsilon turbulence model constants
c
c-----------------------------------------------------------------------
      module turbKE

      real*8 ke_C_mu, ke_C_eps1, ke_C_eps2
      real*8 ke_sigma, ke_otherstuff
      parameter (
     &     ke_C_mu       = 0.09,
     &     ke_C_eps1     = 1.44,
     &     ke_C_eps2     = 1.92,
     &     ke_sigma      = 1.3,
     &     ke_otherstuff = 1.50d0
     &     )

      end module


      LOGICAL FUNCTION ISPRIME(N)	!Ad-hoc, since N is not going to be big...
       INTEGER N			!Despite this intimidating allowance of 32 bits...
       INTEGER F			!A possible factor.
        ISPRIME = .FALSE.		!Most numbers aren't prime.
        DO F = 2,SQRT(DFLOAT(N))	!Wince...
          IF (MOD(N,F).EQ.0) RETURN	!Not even avoiding even numbers beyond two.
        END DO				!Nice and brief, though.
        ISPRIME = .TRUE.		!No factor found.
      END FUNCTION ISPRIME		!So, done. Hopefully, not often.

      PROGRAM CHASE
      INTEGER P1,P2,P3	!The three primes to be tested.
      INTEGER H3,D	!Assistants.
      INTEGER MSG	!File unit number.
      MSG = 6		!Standard output.
      WRITE (MSG,1)	!A heading would be good.
    1 FORMAT ("Carmichael numbers that are the product of three primes:"
     & /"    P1  x P2  x P3 =",9X,"C")
      DO P1 = 2,61	!Step through the specified range.
        IF (ISPRIME(P1)) THEN	!Selecting only the primes.
          DO H3 = 2,P1 - 1		!For 1 < H3 < P1.
            DO D = 1,H3 + P1 - 1		!For 0 < D < H3 + P1.
              IF (MOD((H3 + P1)*(P1 - 1),D).EQ.0	!Filter.
     &        .AND. (MOD(H3 + MOD(-P1**2,H3),H3) .EQ. MOD(D,H3))) THEN	!Beware MOD for negative numbers! MOD(-P1**2, may surprise...
                P2 = 1 + (P1 - 1)*(H3 + P1)/D	!Candidate for the second prime.
                IF (ISPRIME(P2)) THEN		!Is it prime?
                  P3 = 1 + P1*P2/H3			!Yes. Candidate for the third prime.
                  IF (ISPRIME(P3)) THEN			!Is it prime?
                    IF (MOD(P2*P3,P1 - 1).EQ.1) THEN		!Yes! Final test.
                      WRITE (MSG,2) P1,P2,P3, INT8(P1)*P2*P3		!Result!
    2                 FORMAT (3I6,I12)
                    END IF
                  END IF
                END IF
              END IF
            END DO
          END DO
        END IF
      END DO
      END


       subroutine fem_shapefunctions_2r
!***********************************************************************
! Copyright 2011 Los Alamos National Security, LLC  All rights reserved
! Unless otherwise indicated,  this information has been authored by an
! employee or employees of the Los Alamos National Security, LLC (LANS),
! operator of the  Los  Alamos National  Laboratory  under Contract  No.
! DE-AC52-06NA25396  with  the U. S. Department  of  Energy.  The  U. S.
! Government   has   rights  to  use,  reproduce,  and  distribute  this
! information.  The  public may copy  and  use this  information without
! charge, provided that this  Notice and any statement of authorship are
! reproduced on all copies.  Neither  the  Government nor LANS makes any
! warranty,   express   or   implied,   or   assumes  any  liability  or
! responsibility for the use of this information.
!***********************************************************************
!
! Computes the shapefunctions and their derivatives at each gausspoint
! for each quadrilateral element in two dimensions. Called when 'fem'
! computations are used in 2-D.
!
! Author : Sai Rapaka
!

       use comai, only: icnl, nei, ns
       use combi, only: cord
       use comfem

       implicit none

       real*8, dimension(4)   :: N, dNdZeta, dNdEta, dNdMu
       real*8, dimension(4)   :: xx, yy
       real*8, dimension(2,2) :: invJ

       real*8 zeta, eta, mu
       real*8 dXdZeta, dXdEta
       real*8 dYdZeta, dYdEta
       real*8 m11, m12, m13, m21, m22, m23, m31, m32, m33
       real*8 o4, dum
       integer i,j,k

       if(icnl.ne.1) return
       if(ns.ne.4)   return
       !if(ifem.ne.1) return

       do i=1,nei

         do k=1,4
           xx(k) = cord(elnode(i,k),1)
           yy(k) = cord(elnode(i,k),2)
         enddo

         do j=1,numgausspoints
           zeta = gpcord(j,1)
           eta  = gpcord(j,2)

           o4 = 1.0d0/4.0d0
           N(1) = o4*(1 - zeta)*(1 - eta)
           N(2) = o4*(1 + zeta)*(1 - eta)
           N(3) = o4*(1 + zeta)*(1 + eta)
           N(4) = o4*(1 - zeta)*(1 + eta)

           dNdZeta(1) = -o4*(1 - eta)
           dNdZeta(2) = +o4*(1 - eta)
           dNdZeta(3) = +o4*(1 + eta)
           dNdZeta(4) = -o4*(1 + eta)

           dNdEta(1) = -o4*(1 - zeta)
           dNdEta(2) = -o4*(1 + zeta)
           dNdEta(3) = +o4*(1 + zeta)
           dNdEta(4) = +o4*(1 - zeta)

           dXdZeta = 0.0d0
           dXdEta  = 0.0d0
           dYdZeta = 0.0d0
           dYdEta  = 0.0d0

           do k=1,4
             dXdZeta = dXdZeta + xx(k)*dNdZeta(k)
             dXdEta  = dXdEta  + xx(k)*dNdEta(k)

             dYdZeta = dYdZeta + yy(k)*dNdZeta(k)
             dYdEta  = dYdEta  + yy(k)*dNdEta(k)
           enddo

           dum = dXdZeta*dYdEta - dXdEta*dYdZeta
           detJ(i,j) = dum

           ! minors
           m11 = dYdEta
           m12 = dXdEta
           m21 = dYdZeta
           m22 = dXdZeta

           ! inverse of jacobian matrix
           invJ(1,1) = +m11
           invJ(1,2) = -m21
           invJ(2,1) = -m12
           invJ(2,2) = +m22

           invJ = invJ/detJ(i,j)

           do k = 1,ns
             Psi(i,j,k) = N(k)
             dPsidX(i,j,k) = invJ(1,1)*dNdZeta(k) + invJ(1,2)*dNdEta(k)
             dPsidY(i,j,k) = invJ(2,1)*dNdZeta(k) + invJ(2,2)*dNdEta(k)
           enddo

         enddo

       enddo

       end subroutine fem_shapefunctions_2r


       PARAMETER (NSTEP=20)
       DIMENSION PHI(0:NSTEP),S(0:NSTEP)

50     PRINT *,' Enter omega (.le. 0 to stop)'
       READ *, OMEGA
       IF (OMEGA .LE. 0) STOP

       H=1./NSTEP

       DO 10 IX=0,NSTEP
          X=IX*H
          S(IX)=H*H*12*X*X
          PHI(IX)=0.
10     CONTINUE

       DO 20 ITER=1,500
          DO 15 IX=1,NSTEP-1
             PHIP=(PHI(IX-1)+PHI(IX+1)+S(IX))/2
             PHI(IX)=(1.-OMEGA)*PHI(IX)+OMEGA*PHIP
15        CONTINUE
          IF (MOD(ITER-1,20) .EQ. 0) THEN
             E=0.
             DO 30 IX=1,NSTEP
                E=E+(((PHI(IX)-PHI(IX-1))/H)**2)/2
                E=E-S(IX)*PHI(IX)/H**2
30           CONTINUE
             E=E*H
             PRINT *,' iteration = ',iter,'  energy = ',E
          END IF
20      CONTINUE

        GOTO 50
        END


SUBROUTINE DiffYEven_CD(var,varY,IndexesY,StencilsY,Nx,Ny,Nz,beta)
!
! Explicit order differentiation in Y-direction.
!
! By Allan P. Engsig-Karup.
USE Precision
USE DataTypes
IMPLICIT NONE
TYPE (Diff_def) :: DiffStencils
INTEGER :: Nx, Ny, Nz, rank, order, beta,i,j,k
REAL(KIND=long), DIMENSION(Nz,Nx,Ny) :: var, varY
! GD: addition for correct cross derivatives near corners...
INTEGER, DIMENSION(Nz,Nx,Ny,2*beta+1) :: IndexesY
REAL(KIND=long), DIMENSION(Nz,Nx,Ny,2*beta+1) :: StencilsY
INTEGER :: ii
rank = 2*beta+1

DO j = 1, Ny
	DO i = 1, Nx
		DO k = 1, Nz
			varY(k,i,j) = DOT_PRODUCT(StencilsY(k,i,j,1:rank),var(k,i,IndexesY(k,i,j,1:rank)))
		END DO
	END DO
END DO

END SUBROUTINE DiffYEven_CD


!-----------------------------------------------------------------------
!
! Header with QFunctions
!
      include 't540-operator-f.h'
!-----------------------------------------------------------------------
      program test
      implicit none
      include 'ceed/fortran.h'

      integer ceed,err,i,j
      integer stridesx(3),stridesu(3),stridesq(3)
      integer erestrictxi,erestrictui,erestrictqi
      integer bx,bu
      integer qf_setup_mass,qf_apply
      integer op_setup_mass,op_apply,op_inv
      integer qdata_mass,x,u,v
      integer nelem,p,q,d
      parameter(nelem=1)
      parameter(p=4)
      parameter(q=5)
      parameter(d=2)
      integer ndofs,nqpts
      parameter(ndofs=p*p)
      parameter(nqpts=nelem*q*q)
      real*8 arrx(d*nelem*2*2),uu(ndofs)
      integer*8 xoffset,uoffset

      character arg*32

      external setup_mass,apply

      call getarg(1,arg)

      call ceedinit(trim(arg)//char(0),ceed,err)

! DoF Coordinates
      do i=0,1
        do j=0,1
          arrx(i+1+j*2+0*4)=i
          arrx(i+1+j*2+1*4)=j
        enddo
      enddo
      call ceedvectorcreate(ceed,d*nelem*2*2,x,err)
      xoffset=0
      call ceedvectorsetarray(x,ceed_mem_host,ceed_use_pointer,arrx,xoffset,err)

! Qdata Vector
      call ceedvectorcreate(ceed,nqpts,qdata_mass,err)

! Restrictions
      stridesx=[1,2*2,2*2*d]
      call ceedelemrestrictioncreatestrided(ceed,nelem,2*2,d,d*nelem*2*2,&
     & stridesx,erestrictxi,err)

      stridesu=[1,p*p,p*p]
      call ceedelemrestrictioncreatestrided(ceed,nelem,p*p,1,ndofs,&
     & stridesu,erestrictui,err)

      stridesq=[1,q*q,q*q]
      call ceedelemrestrictioncreatestrided(ceed,nelem,q*q,1,nqpts,&
     & stridesq,erestrictqi,err)

! Bases
      call ceedbasiscreatetensorh1lagrange(ceed,d,d,2,q,ceed_gauss,bx,err)
      call ceedbasiscreatetensorh1lagrange(ceed,d,1,p,q,ceed_gauss,bu,err)

! QFunction - setup mass
      call ceedqfunctioncreateinterior(ceed,1,setup_mass,&
     &SOURCE_DIR&
     &//'t540-operator.h:setup_mass'//char(0),qf_setup_mass,err)
      call ceedqfunctionaddinput(qf_setup_mass,'dx',d*d,ceed_eval_grad,err)
      call ceedqfunctionaddinput(qf_setup_mass,'weight',1,ceed_eval_weight,err)
      call ceedqfunctionaddoutput(qf_setup_mass,'qdata',1,ceed_eval_none,err)

! Operator - setup mass
      call ceedoperatorcreate(ceed,qf_setup_mass,ceed_qfunction_none,&
     & ceed_qfunction_none,op_setup_mass,err)
      call ceedoperatorsetfield(op_setup_mass,'dx',erestrictxi,&
     & bx,ceed_vector_active,err)
      call ceedoperatorsetfield(op_setup_mass,'weight',&
     & ceed_elemrestriction_none,bx,ceed_vector_none,err)
      call ceedoperatorsetfield(op_setup_mass,'qdata',erestrictqi,&
     & ceed_basis_collocated,ceed_vector_active,err)

! Apply Setup Operators
      call ceedoperatorapply(op_setup_mass,x,qdata_mass,&
     & ceed_request_immediate,err)

! QFunction - apply
      call ceedqfunctioncreateinterior(ceed,1,apply,&
     &SOURCE_DIR&
     &//'t540-operator.h:apply'//char(0),qf_apply,err)
      call ceedqfunctionaddinput(qf_apply,'u',1,ceed_eval_interp,err)
      call ceedqfunctionaddinput(qf_apply,'mass qdata',1,ceed_eval_none,err)
      call ceedqfunctionaddoutput(qf_apply,'v',1,ceed_eval_interp,err)

! Operator - apply
      call ceedoperatorcreate(ceed,qf_apply,ceed_qfunction_none,&
     & ceed_qfunction_none,op_apply,err)
      call ceedoperatorsetfield(op_apply,'u',erestrictui,&
     & bu,ceed_vector_active,err)
      call ceedoperatorsetfield(op_apply,'mass qdata',erestrictqi,&
     & ceed_basis_collocated,qdata_mass,err)
      call ceedoperatorsetfield(op_apply,'v',erestrictui,&
     & bu,ceed_vector_active,err)

! Apply original operator
      call ceedvectorcreate(ceed,ndofs,u,err)
      call ceedvectorsetvalue(u,1.d0,err)
      call ceedvectorcreate(ceed,ndofs,v,err)
      call ceedvectorsetvalue(v,0.d0,err)
      call ceedoperatorapply(op_apply,u,v,ceed_request_immediate,err)

! Create FDM element inverse
      call ceedoperatorcreatefdmelementinverse(op_apply,op_inv,&
     & ceed_request_immediate,err)

! Apply FDM element inverse
      call ceedoperatorapply(op_inv,v,u,ceed_request_immediate,err)

! Check Output
      call ceedvectorgetarrayread(u,ceed_mem_host,uu,uoffset,err)
      do i=1,ndofs
        if (abs(uu(uoffset+i)-1.0)>5.0d-14) then
! LCOV_EXCL_START
          write(*,*) '[',i,'] Error in inverse: ',uu(uoffset+i),' != 1.0'
! LCOV_EXCL_STOP
        endif
      enddo
      call ceedvectorrestorearrayread(u,uu,uoffset,err)

! Cleanup
      call ceedqfunctiondestroy(qf_setup_mass,err)
      call ceedqfunctiondestroy(qf_apply,err)
      call ceedoperatordestroy(op_setup_mass,err)
      call ceedoperatordestroy(op_apply,err)
      call ceedoperatordestroy(op_inv,err)
      call ceedelemrestrictiondestroy(erestrictxi,err)
      call ceedelemrestrictiondestroy(erestrictui,err)
      call ceedelemrestrictiondestroy(erestrictqi,err)
      call ceedbasisdestroy(bu,err)
      call ceedbasisdestroy(bx,err)
      call ceedvectordestroy(x,err)
      call ceedvectordestroy(u,err)
      call ceedvectordestroy(v,err)
      call ceedvectordestroy(qdata_mass,err)
      call ceeddestroy(ceed,err)
      end
!-----------------------------------------------------------------------


!cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc
!                                                             c
!     Subroutine Flash1                                       c
!     SORTS ARRAY A WITH N ELEMENTS BY USE OF INDEX VECTOR L  c
!     OF DIMENSION M WITH M ABOUT 0.1 N.                      c
!     Karl-Dietrich Neubert, FlashSort1 Algorithm             c
!     in  Dr. Dobb's Journal Feb.1998,p.123                   c
!                                                             c
!cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc

      subroutine flash1 (A, N, L, M, ind)

      implicit none
      double precision :: a(*), anmin, c1, hold, flash
      integer :: L(*), ind(*), i, n, nmax, m, k, ihold, nmove, j, iflash
!     ============================ CLASS FORMATION =====

      do i = 1, n
      ind(i) = i
      end do

      ANMIN=A(1)
      NMAX=1
      DO I=1,N
         IF( A(I).LT.ANMIN) ANMIN=A(I)
         IF( A(I).GT.A(NMAX)) NMAX=I
      END DO

      IF (ANMIN.EQ.A(NMAX)) RETURN
      C1=(M - 1) / (A(NMAX) - ANMIN)
      DO K=1,M
         L(K)=0
      END DO
      DO I=1,N
         K=1 + INT(C1 * (A(I) - ANMIN))
         L(K)=L(K) + 1
      END DO
      DO K=2,M
         L(K)=L(K) + L(K - 1)
      END DO
      HOLD=A(NMAX)
      A(NMAX)=A(1)
      A(1)=HOLD

      ihold = ind(nmax)
      ind(nmax) = ind(1)
      ind(1) = ihold

!     =============================== PERMUTATION =====
      NMOVE=0
      J=1
      K=M
      DO WHILE (NMOVE.LT.N - 1)
         DO WHILE (J.GT.L(K))
            J=J + 1
            K=1 + INT(C1 * (A(J) - ANMIN))
         END DO
         FLASH=A(J)
         iflash=ind(j)

         DO WHILE (.NOT.(J.EQ.L(K) + 1))
            K=1 + INT(C1 * (FLASH - ANMIN))
            HOLD=A(L(K))
            ihold = ind(L(k))
            A(L(K))=FLASH
            ind(L(k)) = iflash
            iflash = ihold
            FLASH=HOLD
            L(K)=L(K) - 1
            NMOVE=NMOVE + 1
         END DO
      END DO

!     ========================= STRAIGHT INSERTION =====
      DO I=N-2,1,-1
         IF  (A(I + 1).LT.A(I)) THEN
            HOLD=A(I)
            ihold = ind(i)
            J=I
            DO WHILE  (A(J + 1).LT.HOLD)
               A(J)=A(J + 1)
               ind(j) = ind(j+1)
               J=J + 1
            END DO
            A(J)=HOLD
            ind(j) = ihold
         ENDIF
      END DO

!     =========================== RETURN,END FLASH1 =====
      RETURN
      END