************************************************************************
* Computes the norms RESU, RESDIV
*
* This routine computes the "U-residual" RESU and the "divergence
* residual" RESDIV.
*
* In:
*   U1,
*   U2,
*   P      - Velocity/pressure vector of the solution of
*            the linearised system    A * (U1,U2,P) = (F1,F2,FP)
*   D1,
*   D2,
*   DP     - Defect vector   (D1,D2,DP) = (F1,F2,FP) - A*(U1,U2,P)
*   F1,
*   F2,
*   FP     - RHS vector of the system   A * (U1,U2,P) = (F1,F2,FP)
*   NU     - length of the velocity vectors
*   NP     - length of the pressure vectors
*
* Out:
*   RESU   - Norm of the velocity residual:
*                               || (D1,D2) ||_E
*               RESU   = -----------------------------
*                        max ( ||F1||_E , ||F2||_E )
*
*   RESDIV - Norm of the pressure residual
*                           || P ||_E
*               RESDIV = ----------------
*                        || (U1,U2) ||_E
************************************************************************

      SUBROUTINE RESDFK(U1,U2,P,D1,D2,DP,F1,F2,FP,NU,NP,RESU,RESDIV)

      IMPLICIT NONE

C parameters

      INTEGER NU,NP
      DOUBLE PRECISION U1(*),U2(*),P(*),D1(*),D2(*),DP(*),F1(*),F2(*),
     *                 FP(*)
      DOUBLE PRECISION RESU, RESDIV

C local variables

      DOUBLE PRECISION RESF, RESF1, RESF2, RESU1, RESU2
      DOUBLE PRECISION DNORMU, DNRMU1, DNRMU2

C-----------------------------------------------------------------------
C     Compute the relative l2-norms  RESU,RESDIV
C-----------------------------------------------------------------------

C     RESF := max ( ||F1||_E , ||F2||_E )

      CALL LL21 (F1,NU,RESF1)
      CALL LL21 (F2,NU,RESF2)
      RESF=MAX(RESF1,RESF2)
      IF (ABS(RESF).LT.1D-8) RESF=1D0

C                   || (D1,D2) ||_E
C     RESU = -----------------------------
C            max ( ||F1||_E , ||F2||_E )

      CALL LL21 (D1,NU,RESU1)
      CALL LL21 (D2,NU,RESU2)
      RESU=SQRT(RESU1*RESU1+RESU2*RESU2)/RESF

C     DNORMU = || (U1,U2) ||_l2

      CALL LL21 (U1,NU,DNRMU1)
      CALL LL21 (U2,NU,DNRMU2)
      DNORMU=SQRT(DNRMU1*DNRMU1+DNRMU2*DNRMU2)
      IF (ABS(DNORMU).LT.1D-8) DNORMU=1D0

C                 || P ||_E
C     RESDIV = ----------------
C              || (U1,U2) ||_E

      CALL LL21 (DP,NP,RESDIV)
      RESDIV=RESDIV/DNORMU
C
      END

************************************************************************
* Defect-RHS and matrix construction routine 4
*
* Extended calling convention
*
* Build the linear part of the defect vector for the nonlinear
* iteration. Combinate Stokes- and Mass-matrix to build the
* linear part of the nonlinear system matrix.
*
* This routine is called in NSDEF2 to assemble mass-/system
* matrices for the nonlinear iteration. It has the same duty as
* XMADF2 but uses a different calling convention.
*
* The shortcut nodal property array is used to decide on which
* DOF's are boundary and which not!
*
* In:
*   TRIA   - array [1..SZTRIA] of integer
*            Triangulation structure on current level
*   MATPAR - array [1..SZN2VI] of integer
*            TNS2DMatrixParams structure containing all information about
*            system-/mass-/Laplace matrices on the current level
*   DU     - array [1..2*NEQA+NEQB] of double
*            This is the the X-,Y- and pressure-component of the current
*            solution vector u^n of the equation.
*            Must correspond to MATPAR.
*   DD     - array [1..2*NEQA+NEQB] of double
*            Vector that contains the current RHS-vector of the linear
*            system for velocity and pressure. This will be overwritten
*            by the resulting defect vector, if IDEF=1.
*            Must correspond to MATPAR.
*   THSTEP - double. Theta-scheme identifier.
*            =0: Forward Euler, =1: Backward Euler, =0.5: Crank Nicolson
*            For Fractional-Step theta scheme this is set to different
*            values, depending on the current step in the scheme.
*            For stationary simulations this parameter must be set
*            to 1 to include the full Laplacian matrix into the
*            system matrix:
*                    [alpha*M + THETA*KST1 ]
*   IALPHA - Whether the simulation is stationary or not.
*            As this routine generally builds
*                    [alpha*M + Theta_1*nu*k*L ] u
*            (the linear part of S(u)), this parameter decides whether
*            ALPHA=0 or ALPHA=1, so whether the mass
*            matrix is added or not.
*   IDEF   - Whether to update the defect vector according to
*             DEF := DEF - [ ALPHA*M*I - THSTEP*(-nu*Laplace(u^n)) ] u
*            = 0: don't update defect vector
*            <>0: update defect vector
*   IPRECA - Defines how to set up the system matrix.
*            = 0,
*            = 1: Copy a precalculated Stokes matrix KST to KA
*                 and add mass matrix if desired
*            = 2,
*            = 3: Read precalculated Stokes matrix for level ILEV from
*                 disc and add mass matrix if desired
*            = 4: Clear the System matrix. In a stationary simulation
*                 (IALPHA=1) with lumped mass matrix (IMASS=0),
*                 add the mass matrix to the system matrix.
*   INEUM  - Whether there are Neumann boundary components in our
*            problem or not.
*   ILEV   - Current level. Only used if IPRECA=3 to read the correct
*            matrix into the memory - otherwise ignored.
*
* Out:
*   - The system matrix (identified by MATPAR(ILEV).LA1) is filled with
*     a linear approximation to the nonlinear system matrix
*   - DD   - Will receive the defect vector, calculated
*            with the RHS, the solution and the linear part of the
*            nonlinear system matrix:
*                  DD = DD - [alpha*M*u-THSTEP*{-nu*Laplace(u)}]*u
************************************************************************

      SUBROUTINE XMADF4(TRIA,MATPAR,DU,DD,IDEF,IPRECA,
     *                  INEUM,THSTEP,IALPHA,ILEV)

************************************************************************

      IMPLICIT NONE

C main COMMON blocks

      INCLUDE 'cmem.inc'
      INCLUDE 'cout.inc'
      INCLUDE 'cerr.inc'

      INCLUDE 'cbasicmg.inc'

      INCLUDE 'stria.inc'
      INCLUDE 'slinsol.inc'
      INCLUDE 'slinsol2dns.inc'
      INCLUDE 'smat2dns.inc'

C parameters

      INTEGER TRIA(SZTRIA),MATPAR(SZN2MI)
      INTEGER INEUM,IALPHA,IDEF,IPRECA,ILEV
      DOUBLE PRECISION DU(*),DD(*),THSTEP

C local variables

      INTEGER KM1,KST1,KA1,KCOLA,KLDA,KB1,KB2,KCOLB,KLDB
      INTEGER NA,KSCNPR,NBDMT,NU
      INTEGER INU, INUA

C constants for filenames

      CHARACTER CFILST*12,CFILM*12,CFILE*12
      DIMENSION CFILST(NNLEV),CFILM(NNLEV)
      DATA CFILST/'ns/ST1     ','ns/ST2     ','ns/ST3     ',
     *            'ns/ST4     ','ns/ST5     ','ns/ST6     ',
     *            'ns/ST7     ','ns/ST8     ','ns/ST9     '/
      DATA CFILM /'ns/MA1     ','ns/MA2     ','ns/MA3     ',
     *            'ns/MA4     ','ns/MA5     ','ns/MA6     ',
     *            'ns/MA7     ','ns/MA8     ','ns/MA9     '/
      SAVE CFILST,CFILM,CFILE

C     At first transfer the starting addresses of the matrices
C     to local variables for faster access.
C
C     We assume here, that we only have one system and one pressure
C     matrix structure.

      KM1    = L(MATPAR(OLM))
      KA1    = L(MATPAR(OLA1))
      KCOLA  = L(MATPAR(OLCLA1))
      KLDA   = L(MATPAR(OLLDA1))
      KB1    = L(MATPAR(OLB1))
      KB2    = L(MATPAR(OLB2))
      KCOLB  = L(MATPAR(OLCLB1))
      KLDB   = L(MATPAR(OLLDB1))
      NA     = MATPAR(ONA1)

C     Use the shortcut nodal property array to decide which points are
C     on the boundary, which are Neumann boundary,...

      NBDMT   = TRIA(OTRIUD)
      KSCNPR  = L(TRIA(OTRIUD+1))

C     We known the number of velocity equations by the size of A

      NU = MATPAR(ONEQA)

C     Standard matrix assembling branch:

      IF ((IPRECA.EQ.0).OR.(IPRECA.EQ.1)) THEN

C       We have a Stokes matrix

        KST1   = L(MATPAR(OLST))

C       Decide whether or not to include the mass matrix (for
C       instationary simulation) into the system matrix:

        IF (IALPHA.EQ.1) THEN

C         We have to build the linear part of the nonlinear system
C         matrix with the help of the Stokes- and mass-matrices:
C
C         KA1 = [  M*I  +  THSTEP * N(u)  ]
C             = [  M*I  +  THSTEP * (-nu * Laplace(.))  +  u * grad(.)  ]
C                  ^^^              ^^^^^^^^^^^^^^^^^     ^^^^^^^^^^^
C                 ->KM1                ->KST1             -> ignored here
C
C         Check if we have a real or a lumped mass matrix.

          IF (MATPAR(OIMALMP).EQ.1) THEN

C           We have a real mass matrix in the structure of the
C           system matrix.
C           Now check the Theta-parameter. If it's 0, we can skip the
C           linear combination with the Stokes-matrix. Otherwise
C           simply add the matrices:
C
C                          KM1 + THSTEP*KST1

            IF (THSTEP.NE.0D0) THEN
              CALL LLC1(DWORK(KST1),DWORK(KA1),NA,THSTEP,0D0)
              CALL LLC1(DWORK(KM1 ),DWORK(KA1),NA,1D0,1D0)
            ELSE
              CALL LCP1(DWORK(KM1),DWORK(KA1),NA)
            ENDIF

          ELSE

C           Using lumped mass matrices we have only to tackle
C           the diagonal when adding it to the system matrix.
C           Again add the matrices:
C
C                          KM1 + THSTEP*KST1

            IF (THSTEP.NE.0D0) THEN
              CALL LLC1(DWORK(KST1),DWORK(KA1),NA,THSTEP,0D0)
              DO INU=1,NU
                INUA=KWORK(KLDA+INU-1)-1
                DWORK(KA1+INUA)=DWORK(KA1+INUA)+DWORK(KM1+INU-1)
              END DO
            ELSE
              CALL LCL1(DWORK(KA1),NA)
              DO INU=1,NU
                INUA=KWORK(KLDA+INU-1)-1
                DWORK(KA1+INUA)=DWORK(KM1+INU-1)
              END DO
            ENDIF
          ENDIF
        ELSE

C         The case of a stationary simulation is much easier.
C         Remember, we have to solve a system of the form
C
C             S(u_h)u_h  +  k B p_h  =  g_h,   B^T u_h = 0
C
C         where
C
C             S(u) = [alpha*M + Theta_1*nu*k*L + Theta_2*k*K(u)] u
C
C         is the matrix whose linear parts we are building here.
C         In the stationary approach, there is alpha=0, so adding
C         the mass matrix can be dropped completely!
C
C         Therefore we only have to build
C
C         KA1 = [  THSTEP * (-nu * Laplace(.))  +  u * grad(.)  ]
C                           ^^^^^^^^^^^^^^^^^     ^^^^^^^^^^^
C                              ->KST1             -> ignored here
C
C         what can simply be done by copying KST1 to KA1 with the
C         correct scaling factor.

          CALL LLC1(DWORK(KST1),DWORK(KA1),NA,THSTEP,0D0)

        ENDIF

C       Multiply the solution vector with the built matrix, and
C       subtract it from the RHS-vector (the current setting of KDx)
C       to build the defect vector.
C
C         DEF := DEF - [ ALPHA*M*I - THSTEP*(-nu*Laplace(u^n)) ] u

        IF (IDEF.NE.0) THEN
          CALL  LAX17 (DWORK(KA1),KWORK(KCOLA),KWORK(KLDA),NU,
     *                 DU(1),DD(1),-1D0,1D0)
          CALL  LAX17 (DWORK(KA1),KWORK(KCOLA),KWORK(KLDA),NU,
     *                 DU(1+NU),DD(1+NU),-1D0,1D0)
        ENDIF

      ENDIF

C=======================================================================

C     Check if the matrices are so big that they have to be read in
C     from a file. The duty of this branch is the same as above.

      IF ((IPRECA.EQ.2).OR.(IPRECA.EQ.3)) THEN

C       Read data about level ILEV into memory.

        IF (IALPHA.EQ.1) THEN
          IF (THSTEP.NE.0D0) THEN
            CALL  OF0 (59,CFILST(ILEV),0)
            CFILE='STMAT '
            CALL  ORA1 (DWORK(KA1),CFILE,59,0)
            REWIND(59)
            CLOSE (59)
            IF (IER.NE.0) RETURN
            CALL LLC1(DWORK(KA1),DWORK(KA1),NA,THSTEP,0D0)
          ELSE
            CALL LCL1(DWORK(KA1),NA)
          ENDIF

          IF (MATPAR(OIMALMP).EQ.1) THEN
            CALL  OF0 (59,CFILM(ILEV),0)
            CFILE='MASMAT'
            CALL  ORALC1 (DWORK(KA1),1D0,CFILE,59,0)
            REWIND(59)
            CLOSE (59)
            IF (IER.NE.0) RETURN
          ELSE
            IF (THSTEP.NE.0D0) THEN
              DO INU=1,NU
                INUA=KWORK(KLDA+INU-1)-1
                DWORK(KA1+INUA)=DWORK(KA1+INUA)+DWORK(KM1+INU-1)
              END DO
            ELSE
              DO INU=1,NU
                INUA=KWORK(KLDA+INU-1)-1
                DWORK(KA1+INUA)=DWORK(KM1+INU-1)
              END DO
            ENDIF
          ENDIF
        ELSE
          CALL  OF0 (59,CFILST(ILEV),0)
          CFILE='STMAT '
          CALL  ORA1 (DWORK(KA1),CFILE,59,0)
          REWIND(59)
          CLOSE (59)
          IF (IER.NE.0) RETURN
          CALL LLC1(DWORK(KA1),DWORK(KA1),NA,THSTEP,0D0)
        ENDIF

        IF (IDEF.NE.0) THEN
          CALL  LAX17 (DWORK(KA1),KWORK(KCOLA),KWORK(KLDA),NU,
     *                 DU(1),DD(1),-1D0,1D0)
          CALL  LAX17 (DWORK(KA1),KWORK(KCOLA),KWORK(KLDA),NU,
     *                 DU(1+NU),DD(1+NU),-1D0,1D0)
        ENDIF

      ENDIF

C=======================================================================

C     In this branch we don't rely on any precalculated information
C     in KST1. If we have a lumped mass matrix, we can build a part of
C     the RHS-vector and of KA1 - otherwise simply clear KA1 as is
C     will be calculated later by the caller.

      IF (IPRECA.EQ.4) THEN

        IF (IALPHA.EQ.1) THEN

          IF (MATPAR(OIMALMP).EQ.0) THEN

            IF (IDEF.NE.0) THEN

C             Treat both components separately instead of
C             simultaneously in one loop. That is a little faster
C             as the compiler can unroll it...

              DO INU=1,NU
                DD(INU)= DD(INU)-DWORK(KM1+INU-1)*DU(INU)
              END DO

              DO INU=1,NU
                DD(NU+INU)= DD(NU+INU)-DWORK(KM1+INU-1)*DU(NU+INU)
              END DO

            ENDIF ! (IDEF.NE.0)

            CALL LCL1(DWORK(KA1),NA)

            DO INU=1,NU
              INUA=KWORK(KLDA+INU-1)-1
              DWORK(KA1+INUA)=DWORK(KM1+INU-1)
            END DO

          ELSE

            CALL LCL1(DWORK(KA1),NA)

          ENDIF ! (IMASS.EQ.0)

        ELSE

          CALL LCL1(DWORK(KA1),NA)

        ENDIF ! (IALPHA.EQ.1)

      ENDIF ! (IPRECA.EQ.4)

C     That's it for the velocity part of the system matrix
C
C=======================================================================
C
C     Now turn over to handle the pressure part. Up to now we
C     constructed the defect vector KD1/KD2 only using the velocity
C     part - but of course that's not enough. Also the pressure-part
C     has to be included into the defect calculation.
C
C     So now update the defect vector by
C
C         KD1 = KD1 - KB1*KP
C
C     to get the correct defect for the velocity. Remember, the
C     whole thing we are calculating here is:
C
C        (def1)   (d1)   [ KST1        KB1 ] ( u1 )
C        (def2) = (d2) - [       KST1  KB2 ] ( u2 )
C        (defp)   (dp)   [ KB1^T KB2^T  0  ] ( p  )
C
C     Standard handling: IPRECB<>2

C      IF (IPRECB.NE.2) THEN

C       Subtract KB1*p from KD1 to get the correct defect for U

        CALL  LAX19 (DWORK(KB1),KWORK(KCOLB),KWORK(KLDB),NU,
     *               DU(1+2*NU),DD(1),-1D0,1D0)

C       Subtract KB2*p from KD2 to get the correct defect for V

        CALL  LAX19 (DWORK(KB2),KWORK(KCOLB),KWORK(KLDB),NU,
     *               DU(1+2*NU),DD(1+NU),-1D0,1D0)

C      ELSE
C
C        Special treatment: IPRECB=2=elementwise application.
C        Might not be implemented completely...
C
C        CALL BMUL1 (KWORK(L(LVERT)),KWORK(L(LMID)),KWORK(L(LADJ)),
C     *              DWORK(L(LCORVG)),DU(1+2*NU),DD(1),DD(1+NU)
C     *              NEL,NVT,NMT,-1D0,1D0)
C
C      ENDIF

C=======================================================================

C     Implement Dirichlet-values into the defect.
C     On all Dirichlet-nodes, the defect must be exactly 0.

      IF (IDEF.NE.0) CALL BDRY0 (DD(1),DD(1+NU),KWORK(KSCNPR),NBDMT)

C=======================================================================

C     Finally calculate the defect vector of the pressure by subtracting
C     the velocity components (multiplied with KB) from KDP.
C
C       KDP = KDP  -  KB1^T KU1  -  KB2^T KU2

C      IF (IPRECB.NE.2) THEN

       CALL  LTX19 (DWORK(KB1),KWORK(KCOLB),KWORK(KLDB),NU,
     *              MATPAR(ONEQB),DU(1),DD(1+2*NU),-1D0,1D0)
       CALL  LTX19 (DWORK(KB2),KWORK(KCOLB),KWORK(KLDB),NU,
     *              MATPAR(ONEQB),DU(1+NU),DD(1+2*NU),-1D0,1D0)

C      ELSE

C       Special treatment: IPRECB=2=elementwise application.
C       Might not be implemented completely...

C       WRITE(6,*) 'BTMUL ???'
C       CALL BTMUL1(KWORK(L(LVERT)),KWORK(L(LMID)),KWORK(L(LADJ)),
C     *             DWORK(L(LCORVG)),DU(1),DU(1+NU),DD(1+2*NU),
C     *             NEL,NVT,NMT,-1D0)
C      ENDIF

99998 END

************************************************************************
* Matrix construction routine 6
*
* Extended calling convention
*
* Combinate Stokes- and Mass-matrix to build the
* linear part of the nonlinear system matrix.
*
* This routine is called in NGSTP2 to assemble mass-/system
* matrices for the nonlinear iteration on all lower levels than NLMAX.
*
* It's basically the same routine as XMADF3, except for that it uses
* a different calling convention.
*
* In:
*   MATPAR - array [1..SZN2VI] of integer
*            TNS2DMatrixParams structure containing all information about
*            system-/mass-/Laplace matrices on the current level
*   IPRECA - Defines how to set up the system matrix.
*            = 0,
*            = 1: Copy a precalculated Stokes matrix KST to KA
*                 and add mass matrix if desired
*            = 2,
*            = 3: Read precalculated Stokes matrix for level ILEV from
*                 disc and add mass matrix if desired
*            = 4: Clear the System matrix. In a stationary simulation
*                 (IALPHA=1) with lumped mass matrix (IMASS=0),
*                 add the mass matrix to the system matrix.
*   THSTEP - double. Theta-scheme identifier.
*            =0: Forward Euler, =1: Backward Euler, =0.5: Crank Nicolson
*            For Fractional-Step theta scheme this is set to different
*            values, depending on the current step in the scheme.
*            For stationary simulations this parameter must be set
*            to 1 to include the full Laplacian matrix into the
*            system matrix:
*                    [alpha*M + THETA*KST1 ]
*   IALPHA - Whether the simulation is stationary or not.
*            As this routine generally builds
*                    [alpha*M + Theta_1*nu*k*L ] u
*            (the linear part of S(u)), this parameter decides whether
*            ALPHA=0 or ALPHA=1, so whether the mass
*            matrix is added or not.
*   ILEV   - Current level. Only used if IPRECA=3 to read the correct
*            matrix into the memory - otherwise ignored.
*
* Out:
*   - The system matrix (identified by MATPAR(ILEV).LA1) is filled with
*     a linear approximation to the nonlinear system matrix
************************************************************************

      SUBROUTINE XMADF5(MATPAR,IPRECA,THSTEP,IALPHA,ILEV)

      IMPLICIT NONE

C main COMMON blocks

      INCLUDE 'cmem.inc'
      INCLUDE 'cout.inc'
      INCLUDE 'cerr.inc'

      INCLUDE 'cbasicmg.inc'

      INCLUDE 'stria.inc'
      INCLUDE 'slinsol.inc'
      INCLUDE 'slinsol2dns.inc'
      INCLUDE 'smat2dns.inc'

C parameters

      INTEGER MATPAR(SZN2MI)
      INTEGER IALPHA,IPRECA,ILEV
      DOUBLE PRECISION THSTEP

C local variables

      INTEGER KM1,KST1,KA1,KCOLA,KLDA,NA,NU
      INTEGER INU, INUA

C constants

      CHARACTER CFILST*12,CFILM*12,CFILE*12
      DIMENSION CFILST(NNLEV),CFILM(NNLEV)
      DATA CFILST/'ns/ST1     ','ns/ST2     ','ns/ST3     ',
     *            'ns/ST4     ','ns/ST5     ','ns/ST6     ',
     *            'ns/ST7     ','ns/ST8     ','ns/ST9     '/
      DATA CFILM /'ns/MA1     ','ns/MA2     ','ns/MA3     ',
     *            'ns/MA4     ','ns/MA5     ','ns/MA6     ',
     *            'ns/MA7     ','ns/MA8     ','ns/MA9     '/
      SAVE CFILST, CFILM, CFILE

C     At first transfer the starting addresses of the matrices
C     to local variables for faster access.
C
C     We assume here, that we only have one system and one pressure
C     matrix structure.

      KM1    = L(MATPAR(OLM))
      KA1    = L(MATPAR(OLA1))
      KCOLA  = L(MATPAR(OLCLA1))
      KLDA   = L(MATPAR(OLLDA1))
      NA     = MATPAR(ONA1)
      NU     = MATPAR(ONEQA)

C     Standard matrix assembling branch:

      IF ((IPRECA.EQ.0).OR.(IPRECA.EQ.1)) THEN

C       We have a Stokes matrix

        KST1   = L(MATPAR(OLST))

C       Decide whether or not to include the mass matrix (for
C       instationary simulation) into the system matrix:

        IF (IALPHA.EQ.1) THEN

C         We have to build the linear part of the nonlinear system
C         matrix with the help of the Stokes- and mass-matrices:
C
C         KA1 = [  M*I  +  THSTEP * N(u)  ]
C             = [  M*I  +  THSTEP * (-nu * Laplace(.))  +  u * grad(.)  ]
C                  ^^^              ^^^^^^^^^^^^^^^^^     ^^^^^^^^^^^
C                 ->KM1                ->KST1             -> ignored here
C
C         Check if we have a real or a lumped mass matrix.

          IF (MATPAR(OIMALMP).EQ.1) THEN

C           We have a real mass matrix in the structure of the
C           system matrix.
C           Now check the Theta-parameter. If it's 0, we can skip the
C           linear combination with the Stokes-matrix. Otherwise
C           simply add the matrices:
C
C                          KM1 + THSTEP*KST1

            IF (THSTEP.NE.0D0) THEN
              CALL LLC1(DWORK(KST1),DWORK(KA1),NA,THSTEP,0D0)
              CALL LLC1(DWORK(KM1),DWORK(KA1),NA,1D0,1D0)
            ELSE
              CALL LCP1(DWORK(KM1),DWORK(KA1),NA)
            ENDIF
          ELSE

C           Using lumped mass matrices we have only to tackle
C           the diagonal when adding it to the system matrix.
C           Again add the matrices:
C
C                          KM1 + THSTEP*KST1

            IF (THSTEP.NE.0D0) THEN
              CALL LLC1(DWORK(KST1),DWORK(KA1),NA,THSTEP,0D0)
              DO INU=1,NU
                INUA=KWORK(KLDA+INU-1)-1
                DWORK(KA1+INUA)=DWORK(KA1+INUA)+DWORK(KM1+INU-1)
              END DO
            ELSE
              CALL LCL1(DWORK(KA1),NA)
              DO INU=1,NU
                INUA=KWORK(KLDA+INU-1)-1
                DWORK(KA1+INUA)=DWORK(KM1+INU-1)
              END DO
            ENDIF
          ENDIF
        ELSE

C         The case of a stationary simulation is much easier.
C         Remember, we have to solve a system of the form
C
C             S(u_h)u_h  +  k B p_h  =  g_h,   B^T u_h = 0
C
C         where
C
C             S(u) = [alpha*M + Theta_1*nu*k*L + Theta_2*k*K(u)] u
C
C         is the matrix whose linear parts we are building here.
C         In the stationary approach, there is alpha=0, so adding
C         the mass matrix can be dropped completely!
C
C         Therefore we only have to build
C
C         KA1 = [  THSTEP * (-nu * Laplace(.))  +  u * grad(.)  ]
C                           ^^^^^^^^^^^^^^^^^     ^^^^^^^^^^^
C                              ->KST1             -> ignored here
C
C         what can simply be done by copying KST1 to KA1 with the
C         correct scaling factor.

          CALL LLC1(DWORK(KST1),DWORK(KA1),NA,THSTEP,0D0)

        ENDIF

      ENDIF

C=======================================================================

C     Check if the matrices are so big that they have to be read in
C     from a file. The duty of this branch is the same as above.

      IF ((IPRECA.EQ.2).OR.(IPRECA.EQ.3)) THEN

        IF (THSTEP.NE.0D0) THEN
          CALL  OF0 (59,CFILST(ILEV),0)
          CFILE='STMAT '
          CALL  ORA1 (DWORK(KA1),CFILE,59,0)
          REWIND(59)
          CLOSE (59)
          IF (IER.NE.0) RETURN
          CALL LLC1(DWORK(KA1),DWORK(KA1),NA,THSTEP,0D0)
        ELSE
          CALL LCL1(DWORK(KA1),NA)
        ENDIF

        IF (MATPAR(OIMALMP).EQ.1) THEN
          CALL  OF0 (59,CFILM(ILEV),0)
          CFILE='MASMAT'
          CALL  ORALC1 (DWORK(KA1),1D0,CFILE,59,0)
          REWIND(59)
          CLOSE (59)
          IF (IER.NE.0) RETURN
        ELSE
          IF (THSTEP.NE.0D0) THEN
            DO INU=1,NU
              INUA=KWORK(KLDA+INU-1)-1
              DWORK(KA1+INUA)=DWORK(KA1+INUA)+DWORK(KM1+INU-1)
            END DO
          ELSE
            DO INU=1,NU
              INUA=KWORK(KLDA+INU-1)-1
              DWORK(KA1+INUA)=DWORK(KM1+INU-1)
            END DO
          ENDIF
        ENDIF

      ENDIF
C=======================================================================

C     In this branch we don't rely on any precalculated information
C     in KST1. If we have a lumped mass matrix, we can build a part of
C     the RHS-vector and of KA1 - otherwise simply clear KA1 as is
C     will be calculated later by the caller.

      IF (IPRECA.EQ.4) THEN

        IF (MATPAR(OIMALMP).EQ.0) THEN
          CALL LCL1(DWORK(KA1),NA)
          DO INU=1,NU
            INUA=KWORK(KLDA+INU-1)-1
            DWORK(KA1+INUA)=DWORK(KM1+INU-1)
          END DO
        ELSE
          CALL LCL1(DWORK(KA1),NA)
        ENDIF

      ENDIF

C=======================================================================

99998 END


!> @file sparse_matrix.f90
!! @brief Sparse matrix module
!! @author Innocent Souopgui
!! @details
!! The sparse matrix module provide the sparse matrix data structure
!! and some basic operation
!!
!! For a complete sparse matrix toolbox,
!! http://people.sc.fsu.edu/~jburkardt/f_src/sparsekit/sparsekit.html
!! Most subroutines in this module are borrowed from sparsekit
!<

!> @brief Module sparse matrix data structures and some basic subroutines
!!
!<
module sparse_matrix
    use general_constant
    use debug_tools
implicit none

    !> @brief user defined type for compressed sparse row matrix
    !! Let @a A be the matrix to store
    !! The @a val vector stores the values of the nonzero elements of
    !!   the matrix @a A as they are traversed in a row-wise fashion
    !! The @a col_ind vector stores the column indexes of the elements
    !!   in the @a val vector. If @a val[k] = @a A[i,j], then col_ind[k] = j
    !! The @a row_ptr vector stores the locations in the @a val vector that
    !!   start a row. If @a val[k] = @a A[i,j],
    !!   then row_ptr[i] <= k < row_ptr[i+1].
    !!   By convention, row_ptr[nrow+1] = nnz+1, where nnz is the number
    !!   of nonzeros in the matrix @a A
    !!
    !! @details http://web.eecs.utk.edu/~dongarra/etemplates/node373.html
    !<
    type CSR_matrix
        integer :: nrow = 0 !< Number of rows of the matrix
        integer :: ncol = 0 !< Number of columns of the matrix
        integer :: nnz = 0 !< Number of nonzero elements of the matrix
        integer, dimension(:), pointer :: col_ind => null()
        integer, dimension(:), pointer :: row_ptr => null()
        real(dp), dimension(:), pointer :: val => null()
        !> \brief indicates if the structure as been initialized
        logical :: initialized      = .false.
    end type CSR_matrix

contains

    !> @brief create a CSR matrix from a full matrix and a mask
    !! @param [in] A full matrix to compress
    !! @param [in] M mask matrix indicating masked entries of @a A
    !! @param [in, out] B csr matrix
    !! @details
    !!   @a A and @a M must have the same shape
    !<
    subroutine full2csr(A, M, B)
        real(dp), dimension(:,:), intent(in) :: A
        integer, dimension(:,:), intent(in)  :: M
        type(CSR_matrix), intent(inout) :: B
        !local variables
        integer :: nrow, ncol, nnz, i, j, pos

        if( sum( abs( shape(A)-shape(M) ) )/=0 )then
            call debug(shape(A), 'Shape of the input matrix')
            call debug(shape(M), 'Shape of the input mask  ')
            call stop_program('in to_csr, input matrix and mask have different shapes')
        end if
        nrow = size(A,1)
        ncol = size(A,2)
        nnz  = count( M == UNMASKED )
        call set_csr(B, nrow, ncol, nnz)
        if(nnz>0)then
            pos = 1!position in the val vector
            do i = 1, nrow
                B%row_ptr(i) = pos
                do j = 1, ncol
                    if(M(i,j)==UNMASKED)then
                        B%val(pos) = A(i,j)
                        B%col_ind(pos) = j
                        pos = pos+1
                    end if
                end do
            end do
            B%row_ptr(nrow+1) = nnz+1
        end if
    end subroutine full2csr

    !> @brief convert a CSR matrix to a full matrix
    !! @param [in] A csr matrix to expand
    !! @param [out] B full matrix (output)
    !! @param [out] M (optional) mask matrix indicating masked entries of @a B
    !! @details
    !!   @a A and @a M must have the same shape that corresponds
    !!   to the shape of the CSR matrix
    !<
    subroutine csr2full(A,B,M)
        type(CSR_matrix), intent(inout) :: A
        real(dp), dimension(:,:), intent(in out) :: B
        integer, dimension(:,:), optional, intent(in out)  :: M

        !local variables
        integer :: nrow, ncol, nnz, i, j, k

        nrow = A%nrow
        ncol = A%ncol
        if( sum( abs( shape(B)-[nrow,ncol] ) )/=0 )then
            call debug([A%nrow,A%ncol], 'Shape of the CSR matrix')
            call debug(shape(B), 'Shape of the full matrix  ')
            call stop_program('In to_csr, shapes of matrices differ')
        end if

        if( present(M) )then
            if( sum( abs( shape(M)-[nrow,ncol] ) )/=0 )then
                call debug([A%nrow,A%ncol], 'Shape of the CSR matrix')
                call debug(shape(M), 'Shape of the mask matrix  ')
                call stop_program('In to_csr, shapes of matrices differ')
            end if

            M = MASKED !suppose all masked
            do i = 1, A%nrow
                do k = A%row_ptr(i), A%row_ptr(i+1)-1
                    M(i, A%col_ind(k) ) = UNMASKED
                end do
            end do
        end if

        B = 0.0_dp
        do i = 1, A%nrow
            do k = A%row_ptr(i), A%row_ptr(i+1)-1
                B(i, A%col_ind(k) ) = A%val(k)
            end do
        end do
    end subroutine csr2full

    !> @brief create a CSR matrix from a full matrix and a mask
    !! @param [in, out] A sparse matrix data structure
    !! @param [in] nrow number of row of the matrix
    !! @param [in] ncol number of column of the matrix
    !! @param [in] nnz number of nonzero entry of the matrix
    !<
    subroutine new_csr(A, nrow, ncol, nnz)
        type(CSR_matrix), intent(inout) :: A
        integer, intent(in)             :: nrow, ncol, nnz

        call set_csr(A, nrow, ncol, nnz)
    end subroutine new_csr

    !> @brief Allocates state dynamic space for the csr matrix
    !! @param [in, out] A sparse matrix data structure
    !! @param [in] nrow number of row of the matrix
    !! @param [in] ncol number of column of the matrix
    !! @param [in] nnz number of nonzero entry of the matrix
    !! @details
    !! This routine is used to allocate space compressed sparse matrices.
    !<
    subroutine set_csr(A, nrow, ncol, nnz)
        type(CSR_matrix), intent(inout) :: A
        integer, intent(in)             :: nrow, ncol, nnz
        !local variables
        logical :: ll_allocate, ll_reset

        ll_allocate = .false.
        ll_reset = .false.

        if( (nnz/=0).and.( (nrow==0).or.(ncol==0) ) )then
            call debug( (/nrow, ncol, nnz/), "nrow ncol, nnz =", tag=dALLWAYS )
            call stop_program("In set_csr: if nnz is nonzero, nrow and ncol must all be nonzero")
        end if

        if( (nrow==0).eqv.(ncol==0) )then !all zero or nonzero
            if( (nrow==0).and.(ncol==0) )then ! all zero
                ll_reset = .true.
            else !both nonzero
                if(A%initialized)then
                    !if current size different from required size
                    if( (A%nrow /= nrow).or.(A%ncol /= ncol).or.(A%nnz /= nnz) )then
                        ll_reset = .true.
                        ll_allocate = .true.
                    end if
                else !not initialized
                    ll_allocate = .true.
                end if
            end if
        else
            call debug( (/nrow, ncol/), "nrow ncol =", tag=dALLWAYS )
            call stop_program("In set_csr: nrow ncol should all be zero or nonzero")
        end if

        if(ll_reset)call reset_csr(A) !free space if necessary

        if( ll_allocate )then
            A%nrow = nrow
            A%ncol = ncol
            A%nnz  = nnz
            allocate( A%val(A%nnz), A%col_ind(A%nnz), A%row_ptr(A%nrow+1))
        end if

        if( nnz>0 )then !all nonzero
            A%val = 0.0_cp
            A%col_ind = 0
            A%row_ptr = A%nnz+1
            A%initialized = .true.
        end if

        !call debug_state( td_state, tag=dALLWAYS )
    end subroutine set_csr

    !> @brief Reset crs data structure, free dynamically allocated memory
    !! @param [in, out] A sparse matrix data structure
    !<
    subroutine reset_csr(A)
        type(CSR_matrix), intent(inout) :: A

        if(A%initialized) then
            deallocate( A%val, A%col_ind, A%row_ptr )
        end if
        nullify(A%val, A%col_ind, A%row_ptr)

        A%nrow = 0
        A%ncol = 0
        A%nnz  = 0

        A%initialized = .false.
    end subroutine reset_csr

    !> @brief transpose a CSR matrix
    !! @param [in] A sparse matrix data structure to transpose
    !! @param [in] B sparse matrix data structure for the transposed matrix
    !! @details
    !!   this is not an in-place operation.
    !!   Developper notes
    !!   this subroutine exploits some properties of operations on CSR
    !!   matrix to limit the number of variables. For example, the row_ptr
    !!   of the transposed matrix is used for three different purposes:
    !!     - count the number of nonzero of each row of the transposed
    !!     - mark the position in the data array of the next avaible position
    !!        for each row of the transpose
    !!     - its actual role of row_ptr
    !! Something to keep in mind (reason why I implemented this function):
    !! in fortran (pgf90 14.6) the code
    !! y = matmul( transpose(A), x)
    !! and the code
    !! B = transpose(A); y = matmul(B,x)
    !! do not produce the same results.
    !<
    subroutine csr_transpose(A, B)
        type(CSR_matrix), intent(in) :: A
        type(CSR_matrix), intent(inout) :: B
        !local variables
        integer :: i, j, k, q, l

        call new_csr(B, A%ncol, A%nrow, A%nnz)

        ! Step 1
        ! counting the number of nonzeros in each row of the transpose
        B%row_ptr(1:B%nrow) = 0
        do i = 1, A%nrow
            do k = A%row_ptr(i), A%row_ptr(i+1)-1
                j = A%col_ind(k)
                B%row_ptr(j) = B%row_ptr(j) + 1
            end do
        end do
        ! At this point, it is mandatory that sum (B%row_ptr(1:B%nrow))=B%nnz
        if( sum(B%row_ptr(1:B%nrow))/=B%nnz )call stop_program("In csr_transpose, step 1 went wrong")

        ! Step 2
        ! Compute the location in the data vector of the first element
        ! of each row of the transpose
        do i = B%nrow, 1, -1
            B%row_ptr(i) = B%row_ptr(i+1) - B%row_ptr(i)
        end do
        ! At this point, it is mandatory that B%row_ptr(1)=1
        if(B%row_ptr(1)/=1)call stop_program("In csr_transpose, step 2 went wrong")

        ! Step 3
        ! Actual copy
        do i = 1,A%nrow
            do k = A%row_ptr(i), A%row_ptr(i+1)-1
                q = A%col_ind(k)
                l = B%row_ptr(q) ! next index for the q row
                B%col_ind(l) = i ! colums of B are rows of A
                B%val(l) = A%val(k)
                B%row_ptr(q) = B%row_ptr(q) + 1
            end do
        end do
        ! At this point B%row_ptr contains pointer to the begining of the next row

        ! Step 4, re-shift B%row_ptr to actual row pointers
        do i = B%nrow, 1, -1
            B%row_ptr(i+1) = B%row_ptr(i)
        end do
        B%row_ptr(1)=1
        ! At this point, it is mandatory that B%row_ptr(B%nrow+1)=B%nnz+1
        if(B%row_ptr(B%nrow+1)/=B%nnz+1)call stop_program("In csr_transpose, step 4 went wrong")
    end subroutine csr_transpose

    !> @brief multiply a CSR matrix A by a vector.
    !! @param[in] A matrix under the CSR form
    !! @param[in] x input vector, must be of size A%ncol
    !! @param[out] y outut vector, must be of size A%nrow
    !! @param[in] trans logical transpose flag
    !! @details
    !!   There is no check of the dimensions
    !!   if trans flag is present and set to .true. then
    !!   y = A^t x, the transpose is used
    !!   otherwise
    !!   y = Ax
    !<
    subroutine CSR_matvec(A, x, y, trans)
        type(CSR_matrix), intent(in) :: A
        real(dp), dimension(:), intent(in) :: x
        real(dp), dimension(:), intent(in out) :: y
        logical, optional :: trans
        !local variables
        integer :: i, k
        real(dp) :: t
        logical :: ll_trans

        if( present(trans) )then
            ll_trans = trans
        else
            ll_trans = .false.
        end if

        if(ll_trans) then
            !apply the transposed matrix
            y = 0.0_dp
            do i = 1, A%nrow
                do k = A%row_ptr(i), A%row_ptr(i+1)-1
                    y( A%col_ind(k) ) = y( A%col_ind(k) ) + x( i )*A%val(k)
                end do
            end do
        else
            do i = 1, A%nrow
                !
                !  Compute the inner product of row i with vector X.
                !
                t = 0.0_dp
                do k = A%row_ptr(i), A%row_ptr(i+1)-1
                    t = t + A%val(k) * x( A%col_ind(k) )
                end do
                y(i) = t

            end do
        end if
    end subroutine CSR_matvec

end module sparse_matrix


  subroutine inv(ndim,Amat)

     implicit none

     integer,parameter :: dp=8

     integer           :: i
     integer           :: info

     integer,intent(in):: ndim

!    IPIV   : INTEGER. Array, DIMENSION at least max(1, n). The pivot indices that define
!    the permutation matrix P; row i of the matrix was interchanged with
!    row ipiv(i). Corresponds to the single precision factorization (if info=
!    0 and iter ≥ 0) or the double precision factorization (if info= 0 and
!    iter < 0).
     integer,allocatable   :: ipiv(:)

     complex(dp),parameter :: zone=(1.0d0,0.0d0)

!    Amat  :
!    Overwritten by the factors L and U from the factorization of A = P*L*U;
!    the unit diagonal elements of L are not stored.
!    If iterative refinement has been successfully used (info= 0 and iter≥
!    0), then A is unchanged.
!    If double precision factorization has been used (info= 0 and iter <
!    0), then the array A contains the factors L and U from the factorization
!    A = P*L*U; the unit diagonal elements of L are not stored.
     complex(dp),intent(inout):: Amat(ndim,ndim)

!    Bmat  :
!    Overwritten by the solution matrix X for dgesv, sgesv,zgesv,zgesv.
     complex(dp),allocatable :: Bmat(:,:)

     allocate(ipiv(ndim))
     allocate(Bmat(ndim,ndim))

     ipiv=0

     ! unit matrix
     Bmat= (0d0, 0d0)
     do i=1,ndim
        Bmat(i,i)= zone
     enddo

     call zgesv(ndim,ndim,Amat,ndim,ipiv,Bmat,ndim,info)

     if(info.ne.0)print *,'something wrong with zgesv'

     Amat=Bmat

     return
  end subroutine inv

  !> get the inverse of a real matrix
  subroutine inv_r(ndim,Amat)

     implicit none

     integer,parameter :: dp=8

     integer           :: i
     integer           :: info

     integer,intent(in):: ndim

!    IPIV   : INTEGER. Array, DIMENSION at least max(1, n). The pivot indices that define
!    the permutation matrix P; row i of the matrix was interchanged with
!    row ipiv(i). Corresponds to the single precision factorization (if info=
!    0 and iter ≥ 0) or the double precision factorization (if info= 0 and
!    iter < 0).
     integer,allocatable   :: ipiv(:)

!    Amat  :
!    Overwritten by the factors L and U from the factorization of A = P*L*U;
!    the unit diagonal elements of L are not stored.
!    If iterative refinement has been successfully used (info= 0 and iter≥
!    0), then A is unchanged.
!    If double precision factorization has been used (info= 0 and iter <
!    0), then the array A contains the factors L and U from the factorization
!    A = P*L*U; the unit diagonal elements of L are not stored.
     real(dp),intent(inout):: Amat(ndim,ndim)

!    Bmat  :
!    Overwritten by the solution matrix X for dgesv, sgesv,zgesv,zgesv.
     real(dp),allocatable :: Bmat(:,:)

     allocate(ipiv(ndim))
     allocate(Bmat(ndim,ndim))

     ipiv=0

     ! unit matrix
     Bmat= 0d0
     do i=1,ndim
        Bmat(i,i)= 1d0
     enddo

     call dgesv(ndim,ndim,Amat,ndim,ipiv,Bmat,ndim,info)

     if(info.ne.0)print *,'something wrong with dgesv'

     Amat=Bmat

     return
  end subroutine inv_r


! PR rtl-optimization/49472
! { dg-do compile }
! { dg-options "-O -fcompare-debug -ffast-math" }
subroutine pr49472
  integer, parameter :: n = 3
  real(8) :: a, b, c, d, e (n+1)
  integer :: i
  do i=2, (n+1)
    b = 1. / ((i - 1.5d0) * 1.)
    c = b * a
    d = -b * c / (1. + b * b) ** 1.5d0
    e(i) = d
  end do
  call dummy (e)
end subroutine


program main
    implicit none
    integer :: n
    print "(a)", "Numbers n is "
    read *, n
    if (is_even(n)) then
        print "(a)", "n is even"
    else
        print "(a)", "n is odd"
    end if
contains
    recursive function is_even(n) result(even)
        logical             :: even
        integer, intent(in) :: n
        if (n > 0) then
            even = is_odd(n - 1)
        else if (n == 0) then
            even = .true.
        end if
    end function is_even

    recursive function is_odd(n) result(odd)
        logical             :: odd
        integer, intent(in) :: n
        if (n > 0) then
            odd = is_even(n - 1)
        else if (n == 0) then
            odd = .false.
        end if
    end function is_odd
end program main


    program demo_scale
    implicit none
    real :: x = 178.1387e-4
    integer :: i = 5
       print *, scale(x,i), x*radix(x)**i
    end program demo_scale


      SUBROUTINE proma2(a,b,c,n1,n2,n3)
!********************************************************************
!
!***this routine evaluates a matrix product
!                                                t
!           a(i,j) = b(i,k) * c(j,k)    a = b * c
!
!********************************************************************
      IMPLICIT NONE

      INTEGER (kind=4),INTENT(IN) :: n1,n2,n3
      REAL (kind=8),INTENT(IN) :: b(1:n1,1:n3), c(1:n2,1:n3)
      REAL (kind=8),INTENT(OUT) :: a(1:n1,1:n2)

      a(1:n1,1:n2) = MATMUL(b(1:n1,1:n3),TRANSPOSE(c(1:n2,1:n3)))
      RETURN

      END SUBROUTINE proma2


! DART software - Copyright UCAR. This open source software is provided
! by UCAR, "as is", without charge, subject to all terms of use at
! http://www.image.ucar.edu/DAReS/DART/DART_download
!
! $Id$

program sys_sim4

! Work done during last week of January 2002 (28 Jan init) to investigate
! impacts of sampling error from small ensembles on update for a single
! variable that is exactly observed; applicable to observation variable
! priors. Small sample estimates of variance have approximately normal
! (or is it exactly normal) error distributions. BUT, when one computes
! the updated variance there is a bias. Of course, one must also account
! for the increased uncertainty in the estimate of the mean resulting from
! errors in the computation of the variance.

use random_seq_mod, only : random_seq_type, init_random_seq, random_gaussian, &
   twod_gaussians, random_uniform

implicit none

! version controlled file description for error handling, do not edit
character(len=256), parameter :: source   = &
   "$URL$"
character(len=32 ), parameter :: revision = "$Revision$"
character(len=128), parameter :: revdate  = "$Date$"

type (random_seq_type) :: r
double precision, allocatable :: rnum(:), var_hist(:)
double precision :: sample_mean, sample_sigma_y_p
double precision :: c(2, 2), sum_err_var, sigma_y_p, sigma_y_o, sigma_x_p
double precision :: y_truth, y_o, sample_correl, reg_coef, sample_reg_coef
double precision :: sigma_y_u, y_u, sigma_x_u, x_u, sample_x_u, x_error
double precision :: x_error_var, total_err_var, mean(2), correl, est_err_var
double precision :: temp_sum, pe_2_sum, xx, s_xx, s_xx_2, xx_mean, xx_var
double precision, allocatable :: x_mean(:), x_var(:)
double precision :: sum_sample_mean, sum_sample_sigma, sum_y_u, sum_sigma_y_u, sum_y_u_2
double precision :: sum_err, sum_err_2, inf_sigma_y_u, var_mean, var_var
integer :: n, i, j, n_samples, index

! Initialize repeatable random sequence
call init_random_seq(r)

! Initialize error accumulation for averaging
sum_err_var = 0.0
mean = 0.0; temp_sum = 0.0; pe_2_sum = 0.0;

! For now have a set of free parameters that may be too large
write(*, *) 'Input prior variance of observation variable'
read(*, *) sigma_y_p
write(*, *) 'Input variance of observing instrument'
read(*, *) sigma_y_o
write(*, *) 'Input ensemble size'
read(*, *) n

! Allocate storage for computing sample variance
allocate(rnum(n))

write(*, *) 'Input number of samples for statistics'
read(*, *) n_samples
allocate(var_hist(n_samples))

sum_sample_mean = 0.0
sum_sample_sigma = 0.0
sum_y_u = 0.0; sum_y_u_2 = 0.0
sum_sigma_y_u = 0.0
sum_err = 0.0; sum_err_2 = 0.0

! Loop through the number of samples
do i = 1, n_samples

! Produce a sample of an observation yo by getting truth from prior
! and adding on sample from observatoinal error distribution
   y_truth = random_gaussian(r, dble(0.0), sqrt(sigma_y_p))
   y_o = y_truth + random_gaussian(r, dble(0.0), sqrt(sigma_y_o))

! Compute the sample variance for this ensemble size
   do j = 1, n
      rnum(j) =  random_gaussian(r, dble(0.0), sqrt(sigma_y_p))
   end do

! Compute the sample variance
   call sample_mean_var(rnum, n, sample_mean, sample_sigma_y_p)
   sum_sample_mean = sum_sample_mean + sample_mean
   sum_sample_sigma = sum_sample_sigma + sample_sigma_y_p
   var_hist(i) = sample_sigma_y_p

!   write(*, *) 'sample mean and variance ', real(sample_mean), real(sample_sigma_y_p)

! Compute the updated covariance and mean for y given this observation
   sigma_y_u = (1.0 / (1.0 / sample_sigma_y_p + 1.0 / sigma_y_o))
   y_u = sigma_y_u * (0.0 + y_o / sigma_y_o)

   sum_y_u = sum_y_u + y_u
   sum_y_u_2 = sum_y_u_2 + y_u**2
   sum_sigma_y_u = sum_sigma_y_u + sigma_y_u
   sum_err = sum_err + y_u - y_truth
   sum_err_2 = sum_err_2 + (y_u - y_truth) ** 2

end do

write(*, *)'mean sample mean and var ', real(sum_sample_mean / n_samples), real(sum_sample_sigma / n_samples)
write(*, *) 'mean update mean and var ', real(sum_y_u / n_samples), real(sum_sigma_y_u / n_samples)
write(*, *) 'mean err ', real(sum_err / n_samples), real(sum_err_2 / n_samples)

! Correct updated error variance is computed here
inf_sigma_y_u = 1.0 / (1.0 / sigma_y_p + 1.0 / sigma_y_o)
write(*, *) 'infinite sample updated error variance ', real(inf_sigma_y_u)
write(*, *) '---------------------------------'

! Output some ratios of the spread measure
write(*, *) 'ratio of predicted var to correct ', real((sum_sigma_y_u / n_samples)  / inf_sigma_y_u), &
   real(inf_sigma_y_u / (sum_sigma_y_u / n_samples))
write(*, *) 'ratio of actual var to correct ', real((sum_err_2 / n_samples) / inf_sigma_y_u)

! Compute variance of original sampled variance
call sample_mean_var(var_hist, n_samples, var_mean, var_var)
write(*, *) '-----------------------------------'
write(*, *) 'original sample mean and var of var ', real(var_mean), real(var_var)
write(*, *) 'predicted var of var is ', 2.0 * sigma_y_p**4 / (n - 1.0)

if(1 == 1) stop

! Output the expected value of the mean x
write(*, *) 'Expected updated mean is ', sum(x_mean) / n_samples

! Now compute the expected value of the updated variance by doing sampling
! The distribution is a sum of Gaussians with means and variance given
! by x_mean and x_var; each is equally likely. So, do a sample, randomly
! select a 'kernel', then randomly select a value from it.
s_xx = 0.0
s_xx_2 = 0.0
do i = 1, 10 * n_samples
   index = random_uniform(r) * n_samples + 1
   if(index < 1 .or. index > n_samples) then
      write(*, *) 'bad index ', i, index
      stop
   end if
   if(i / 500000 * 500000 == i) then
      write(*, *) 'mean, var ', i, index, real(x_mean(index)), real(x_var(index))
   endif
   xx = random_gaussian(r, x_mean(index), sqrt(x_var(index)))
   s_xx = s_xx + xx
   s_xx_2 = s_xx_2 + xx**2
end do

xx_mean = s_xx / (10.0 * n_samples)
write(*, *) 'Sampled mean is ', xx_mean
write(*, *) 'Expected mean is ', x_u
xx_var = (s_xx_2 - (10.0 * n_samples) * xx_mean**2) / &
   (10.0 * n_samples - 1.0)
write(*, *) 'Sampled variance is ', xx_var
write(*, *) 'Expected raw var is ', sigma_x_u
write(*, *) 'Mean variance is    ', s_xx_2 / (10.0 * n_samples)

if(1 == 1) stop

! Output the mean total_err_var
write(*, *) '----------'
write(*, *) 'Large N Mean total_err_var is ', sigma_x_u
write(*, *) 'Expected sample x variance is ', sum_err_var / n_samples
write(*, *) '----------'

! Using sample for product of correlation and y_0_2
write(*, *) 'Expected error variance is                  ', &
   (temp_sum / n_samples )*(sigma_x_p / sigma_y_p) * (sigma_y_u / sigma_y_o)**2

! Estimate using sample only for correlation
write(*, *) 'Expected error variance, only sample correl ', &
   sigma_x_p * sigma_y_p * (pe_2_sum / n_samples) / (sigma_y_p + sigma_y_o)

est_err_var = ((1.0 - correl**2)**2 / (n - 1.)) * sigma_x_p * sigma_y_p / &
   (sigma_y_p + sigma_y_o)
write(*, *) 'Estimated correl expected variance is       ', est_err_var
write(*, *) '----------'

write(*, *) 'pe_2_mean                   ', pe_2_sum / n_samples
write(*, *) 'expected value of pe_2_mean ', (1.0 - correl**2)**2 / (n - 1.)

write(*, *) '----------'
write(*, *) 'net improvement ', (sigma_x_p - sum_err_var / n_samples) / &
   (sigma_x_p - sigma_x_u)
write(*, *) 'Computed net improvement ', 1. - (pe_2_sum / n_samples) / correl**2

end program sys_sim4

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

subroutine comp_correl(ens, n, correl)

implicit none

integer, intent(in) :: n
double precision, intent(in) :: ens(2, n)
double precision, intent(out) :: correl
double precision :: sum_x, sum_y, sum_xy, sum_x2, sum_y2

sum_x = sum(ens(2, :))
sum_y = sum(ens(1, :))
sum_xy = sum(ens(2, :) * ens(1, :))
sum_x2 = sum(ens(2, :) * ens(2, :))

! Computation of correlation
sum_y2 = sum(ens(1, :) * ens(1, :))

correl = (n * sum_xy - sum_x * sum_y) / &
   sqrt((n * sum_x2 - sum_x**2) * (n * sum_y2 - sum_y**2))

end subroutine comp_correl

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

subroutine sample_mean_var(x, n, mean, var)

implicit none

integer, intent(in) :: n
double precision, intent(in) :: x(n)
double precision, intent(out) :: mean, var

double precision :: sx, s_x2

sx = sum(x)
s_x2 = sum(x * x)
mean = sx / n
var = (s_x2 - sx**2 / n) / (n - 1)

end subroutine sample_mean_var


      subroutine far(y,c,s,r,in,r2,sig,n,p,po,pv,ier,io)
c********************************************
c  Multi level iterative AR fit which loops back to discard lags
c   whose coefficients have prob value of abs(t) statistics < pv
c  Input: y - n observations, p - order in, pv - two sided p-value
c  Output: c - po AR coefficients, s - standard errors of c coeff's
c    r(1),...,r(n) - residuals
c    r2 - R square,  sig - standard deviation of residuals
c    in - array of lag values for significant lags, po - order out
c  Version 3-12-2005
c   ier = 1: A is not positive definite, ier = 2: p < 1, ier = 3: p > n/2
c============================================
      allocatable::a,b,d,w
      integer p,pi,po
      integer in(p)
      real y(n),r(n),s(p),pv
      real*8 c(p),a(:),b(:),w(:),d(:),sy,ssy
      intent(in) y,n,p,pv
      intent(out) c,s,r,in,po
c****************************
c  Trap p
      if(p<1) then
       write(io,*) 'p in AR(p) ',p,' < 1'
       ier=2
       return
        elseif(p>n/2) then
         write(io,*) 'p in AR(p) ',p,' > n/2 ',n/2,' n = ',n
         ier=3
         return
      endif
c-------
      allocate(a(0:p*p-1),b(0:p),d(0:p),w(0:p),stat=ibad)
      if(ibad/=0) then
       write(io,*) 'Unable to allocate work space for arrays in far'
       return
      endif
c  Covariances in a
      call cr(y,a,b,w,sy,ssy,n,p,io)
      do k=1,p
       c(k)=0.d0
       in(k)=k
      enddo
      pi=p
      po=p-1
      dowhile(po>0)
c  Iteration on ols
       call ar(y,c,s,r,a,b,d,w,n,p,sy,ssy,r2,sig,pv,ier,in,pi,po,io)
       if(po<pi) then
        pi=po
         else
          exit
       endif
      enddo
c-------
      deallocate(a,b,d,w)
      return
      end
c
c============================================
c
      subroutine ar(y,c,s,r,a,b,d,w,n,p,sy,ssy,r2,sig,pv,ier,in,pi,po,io
     *)
c********************************************
c  Computes estimates of parameters of an AR(p) model and the
c   residuals of the OLS fit.
c  Input: y - data array of n observations, order p
c  Output: r - array of e(k)=y(k)-c1*y(k-1)-c2*y(k-1) ...
c   c - p AR coefficients, s - standard errors of coefficients
c   sig- mean and standard deviation of residuals, r2 - R squared
c   po - Number of p values of t statistics < pv
c============================================
      integer p,pi,po,t,n
      integer in(p)
      real y(n),r(n),s(p),sig,pv
      real*8 a(p*p),b(p),d(p),w(p),c(p),sy,ssy,sse,sm,hd,dy,dnp
      intent(in) y,n,p,pi,pv,sy,ssy
      intent(out) c,s,r,sig,r2,po
      intent(in out) in
c********************************************
c  Cholesky decomposition of a
      call dcholdc(a,pi,d,ier,io)
      if(ier/=0) then
       write(io,*) 'DCHOLDC failed, A is not positive definite'
       write(io,*) 'pi = ',pi
       return
      endif
c-------
      call dcholsl(a,pi,d,b,c)
c  Compute residuals, mean, and standard deviation
      sse=0.d0
      hd=0.
      do t=in(pi)+1,n
       sm=0.d0
       do k=1,pi
        sm=sm+c(k)*(dble(y(t-in(k)))-sy)
       enddo
       dy=dble(y(t))-sy-sm
       r(t)=sngl(dy)
       hd=hd+dy
       sse=sse+dy*dy
      enddo
c  Zero out first pi residuals
      do t=1,in(pi)
       r(t)=0.
      enddo
      dnp=dfloat(n-pi)
      hd=hd/dnp
      sse=sse-dnp*hd*hd
c  R squared
      r2=1.-sngl(sse/ssy)
      r2=r2-float((pi-1)/(n-pi))*(1-r2)
      sig=sngl(dsqrt(sse/dfloat(n-pi-1)))
c-------
c  Inverse of A
      call dinverse(a,pi,d)
c  Standard errors of c's
      prb=1.-pv
      po=0
      do k=1,pi
       s(k)=sig*sngl(dsqrt(a((k-1)*pi+k)))
       ch=sngl(c(k))/s(k)
       call cdchi(ch*ch,1.,pc,io)
       if(pc>prb) then
        po=po+1
        in(po)=in(k)
       endif
      enddo
c-------
      if(po<pi .and. po>0) then
c  Reduce down the covariance matrix
       call reduce(a,b,w,ssy,p,po,in,io)
      endif
c-------
      return
      end
c
c===========================================
c
      subroutine cr(y,a,b,w,sy,ssy,n,p,io)
c********************************************
c  Covariances of y in a & variances in b,w
c  sy - Mean of y, ssy - Sum of squares of y
c============================================
      integer p,t
      real y(n)
      real*8 a(p*p),b(p),w(p),sy,ssy,ac
      intent(in) y,n,p
      intent(out) a,b,w,sy,ssy
c********************************************
c  Compute mean
      sy=0.d0
      do t=1,n
       sy=sy+dble(y(t))
      enddo
      sy=sy/dfloat(n)
c-------
c  Compute sums
      ssy=(dble(y(n))-sy)*(dble(y(n))-sy)
      do k=1,p
       ac=0.d0
c  Compute covariances
       do t=1,n-k
        ac=ac+(dble(y(t))-sy)*(dble(y(t+k))-sy)
        if(k==1) then
         ssy=ssy+(dble(y(t))-sy)*(dble(y(t))-sy)
        endif
       enddo
       b(k)=ac
       w(k)=ac
      enddo
c-------
c  Put in upper triangle of matrix a
      a(p*p)=ssy
      do j=1,p-1
       do k=j+1,p
        a((k-1)*p+j)=b(k-j)
       enddo
       a((j-1)*p+j)=ssy
      enddo
c-------
      return
      end
c
c===========================================
c
      subroutine reduce(a,b,w,ssy,p,po,in,io)
c********************************************
c  Rearrange a & b for reduced least squares fit
c============================================
      integer p,po
      integer in(p)
      real*8 a(p*p),b(p),w(p),ssy
      intent(in) w,ssy,in,p,po
      intent(out) a,b
c********************************************
      do k1=1,po
       b(k1)=w(in(k1))
       a((k1-1)*po+k1)=ssy
       if(po>0) then
        do k2=k1+1,po
         a((k2-1)*po+k1)=w(in(k2)-in(k1))
        enddo
       endif
      enddo
c-------
      return
      end


program c
    use utilities
    use percolation

    implicit none

    integer :: i,j,k, fileunit
    integer, parameter :: L = 1024, num_probabilities = 50, num_samples = 50
    real(kind=dp), dimension(:), allocatable :: probabilities, spanning_densities
    real(kind=dp), dimension(:), allocatable :: estimated_densities
    character(len=:), allocatable :: fmtstring, filename
    real(kind=dp) :: beta, const

    probabilities = linspace(pc + 0.000001d0, 1.02d0*pc, num_probabilities)

    spanning_densities = [ (spanning_density(probabilities(i), L, num_samples), &
                            i = 1, num_probabilities) ]

    call linfit(log10(probabilities - pc), log10(spanning_densities), beta, const)

    estimated_densities = (probabilities - pc)**beta * 10**const

    fmtstring = "(f0.6,x,f0.6,x,f0.6)"
    filename = "tmp/c_P.dat"
    open(newunit=fileunit, file=filename, status="replace")

    do i=1, num_probabilities
        write(unit=fileunit, fmt=fmtstring) probabilities(i), spanning_densities(i), &
                                            estimated_densities(i)
    end do
    close(unit=fileunit)

    open(newunit=fileunit, file="tmp/c_beta.dat", status="replace")
    write(unit=fileunit, fmt="(f0.3)") beta
    close(unit=fileunit)

end program c


program functiontests
      use prep
      use detgen
      use projection
      use gramschmidt, only: prtmtx, mgs, mult, orth

      implicit none

      integer(i16b) :: det(2)
      integer:: i, j, n, m, k
      real(rk), allocatable ::z,  A(:, :), Q(:, :), R(:, :)
      !Lz_det Szt2_det tested, correct.
      !det(1) = 0
      !det(2) = 0
      !do i = 1, 10
      !    det(1) = det(1) + 1
      !    do j = 1, 10
      !        det(2) = det(2) + 1
      !        write(*, '(2b16, 2I8)') det(1:2), Lz_det(det), Szt2_det(det)
      !    enddo
      !enddo
     ! n = 2
     ! m = 8
     ! allocate(A(n, n), Q(n, n), R(n, n))
     ! A(1, :) = (/-1., 1./)
     ! A(2, :) = (/2., -2./)

      !A(1, :) = (/1., 2., 3., 4./)
      !A(2, :) = (/5.,6.,7.,8./)
      !A(3, :) = (/0.,9.,10.,11./)
      !A(4, :) = (/0.,0.,12.,13./)
     ! call orth(A, Q, R, m, n)

end program functiontests


C NCLFORTSTART
      subroutine gamfitd3 (datarr, n, dmsg, pcrit, invscale
     +                    ,alpha , scale, shape, pzero,ier)
cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc
c based upon: www.unicam.it/matinf/pasef/docs/CDIIIsem/home/pidieffe/spi.pdf
cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc
c Estimate incomplete gamma parameters.
c
c Input:
c	datarr - data array
c	n      - size of datarr
c	dmsg   - missing data
c	pcrit  - scalar (int,float,double) specifying % non missing data
c	invscale -  if invscale=1, then return (1/beta)
c
c Output:
c	alpha (???), beta (scale), eta (shape) - gamma parameters
c	pzero - probability of zero.
cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc
      implicit none
c                                                        INPUT
      integer n, invscale,  ier
      double precision datarr(n), dmsg, pcrit
c                                                        OUTPUT
      double precision shape, scale, alpha, pzero
c NCLEND
c                                                        LOCAL
      integer i, nact, nmsg
      double precision    sum, sumlog, av, pobs, pc
      double precision    beta, eta
c     double precision    s, kappa

      shape = dmsg
      scale = dmsg
      alpha = dmsg

      beta  = scale
      eta   = shape
      pzero = dmsg

      ier = 0
      if (n .le. 0) then
          ier   = 1
c c c     write(*, *) 'Error in gamfit - empty data array'
          return
      end if

      sum    = 0.0d0
      sumlog = 0.0d0
      pzero  = 0.0d0
      nact   = 0
      nmsg   = 0

c	compute sums and counts

      do i = 1, n
         if (datarr(i) .ne. dmsg) then
             if (datarr(i) .gt. 0.0d0) then
                 sum    = sum + datarr(i)
                 sumlog = sumlog + log(datarr(i))
                 nact   = nact + 1
             else
                 pzero  = pzero + 1.0d0
             end if
         else
             nmsg = nmsg+1
         end if
      end do
c                             pobs id the total # of non-missing obs
      pobs = nact + pzero
      pc   = 100d0*pobs/n
      if (pc.lt.pcrit) then
          ier = 2
c c c     write(*, *) 'too few observations'
          return
      end if

      pzero = pzero/pobs
      if (nact .gt. 0) av = sum / nact

c	Bogus data array but do something reasonable
      if (nact .eq. 1) then
          alpha = 0.0d0
          eta   = 1.0d0
          beta  = av
          return
      end if

c	They were all zeroes.
      if (pzero .eq. 1) then
          alpha = 0.0d0
          eta   = 1.0d0
          beta  = av
          return
      end if

c	Use MLE  (Maximum Liklihood Estimate)
c       A Note on the Gamma Distribution
c       Thom (1958): Monthly Weather Review, pp 117-122.
c       eqn 22 for gamma; just above eqn 21 "A" => alpha

      alpha = log(av)-(sumlog/nact)
      eta   = (1.0d0+sqrt(1.0d0+4.0d0*alpha/3.0d0))/(4.0d0*alpha)
      beta  = av/eta

c ------------
c http://en.wikipedia.org/wiki/Gamma_distribution#Maximum_likelihood_estimation
c alpha is "s" at the above url
c eta (shape)  is approximately "kappa" at the above url
c       The numbers for eta and kappa are very similar

c c c s     = alpha
c c c kappa = (3-s+sqrt((s-3)**2 + 24*s))/(12*s)
c ------------

      if (invscale.eq.1 .and. beta.gt.0.0d0) then
          beta = 1.0d0/beta
      end if

      shape = eta
      scale = beta

      return
      end


c***************************************************************************
c			OBL_ACC.F
c*************************************************************************
c OBL_ACC returns the BARYCENTRIC x,y,z components of the acc. on NBOD
c particles due to the oblateness of mass(1) using
c the values of J2RP2 and J4RP4 passed into the routine.
c (J2RP2 for example is the product of
c J_2 times the square of the central body's radius)
c Here we return the net acc. produced
c only by the J2 and J4 terms (i.e. including
c neither the monopole nor higher order terms).
c
c
c             Input:
c                 nbod     ==>  number of massive bodies (incl. central one)
c                 mass(*)  ==>  masses of particles (real*8 array)
c                 j2rp2    ==>  scaled value of j2 moment (real*8 scalar)
c                 j4rp4    ==>  scaled value of j4 moment (real*8 scalar)
c                                    (real*8 vectors)
c                 xh(*),yh(*),zh(*)   ==>  HELIO. positions of particles
c                 irh(*)   ==> 1./ magnitude of radius vector (real*8 vector)
c                                (passed in to save calcs.)
c             Output:
c               aoblx(*),aobly(*),aoblz(*)  ==>  BARY. components of accel
c                                        (real*8 vectors)
c
c Remarks:  aoblx(1) (for example) contains x-component of
c           bary. acc. of central body
c Authors:  Martin Duncan
c Date:    3/4/94
c Last revision:

      subroutine obl_acc(nbod,mass,j2rp2,j4rp4,xh,yh,zh,irh,
     &     aoblx,aobly,aoblz)

      include '../swift.inc'

c...  Inputs Only:
      integer nbod
      real*8 j2rp2,j4rp4
      real*8 mass(NPLMAX)
      real*8 xh(NPLMAX),yh(NPLMAX),zh(NPLMAX),irh(NPLMAX)

c...  Output
      real*8 aoblx(NPLMAX),aobly(NPLMAX),aoblz(NPLMAX)

c...  Internals
      integer n
      real*8 rinv2,t0,t1,t2,t3
      real*8 fac1,fac2

c----
c...  executable code

c First get the bary acc. of each "planet" due to central oblate "sun"

	do n =2,nbod

c Note that here we assume we know inverse of radius rather than calc. it
c from (x,y,z) to save the sqrt.
	  rinv2= irh(n)**2
	  t0 = -mass(1)*rinv2*rinv2*irh(n)
	  t1 = 1.5d0 *j2rp2
	  t2 = zh(n)*zh(n)*rinv2
	  t3 = 1.875d0 *j4rp4*rinv2

	  fac1 = t0*(t1 - t3 - (5.d0*t1 - (14.d0 - 21.d0*t2)*t3)*t2)
	  fac2 = 2.d0*t0*(t1 - (2.d0 - (14.d0*t2/3.d0))*t3)

          aoblx(n) = fac1*xh(n)
          aobly(n) = fac1*yh(n)
          aoblz(n) = (fac1 + fac2)*zh(n)

	enddo

c Now compute the bary. acc. of Sun due to all the planets

	aoblx(1) = 0.d0
	aobly(1) = 0.d0
	aoblz(1) = 0.d0
	do n=2,nbod
	   aoblx(1) = aoblx(1) - mass(n)*aoblx(n)/mass(1)
	   aobly(1) = aobly(1) - mass(n)*aobly(n)/mass(1)
	   aoblz(1) = aoblz(1) - mass(n)*aoblz(n)/mass(1)
	enddo

        return
        end                       !  obl_acc.f
c____________________________________________________________________________


      SUBROUTINE CALEBI
C
C CHANGE RECORD
C **  CALEBI CALCULATES THE EXTERNAL BUOYANCY INTEGRALS
C
      USE GLOBAL
	IMPLICIT NONE
	INTEGER::K,L,LLCM
	REAL::DBK,DZCBK

      REAL*4 DZCB(KCM)
      REAL*4 BK(KCM)

      PARAMETER(LLCM=200)
!      REAL*4  BI1T(LLCM)
!      REAL*4  BI2T(LLCM)
!      REAL*4  BET(LLCM)

      DO L=2,LA

        BI1(L)=0.
        BI2(L)=0.
        BE(L)=0.

        DO K=1,KC
          DZCB(K)=DZC(K)*B(L,K)
        ENDDO

        DBK=0.
        DO K=KC,1,-1
          DBK=DBK+DZCB(K)         !DZC(K)*B(L,K)
          BK(K)=DBK-0.5*DZCB(K)   !DZC(K)*B(L,K)
        ENDDO

        !Z(0)=0.
        !Z(K)=Z(K-1)+DZC(K)
        DO K=1,KC
          BE(L) =BE(L)+DZCB(K)   !DZC(K)*B(L,K)
          DZCBK =DZC(K)*BK(K)
          BI1(L)=BI1(L)+DZCBK
          BI2(L)=BI2(L)+(DZCBK+0.5*(Z(K)+Z(K-1))*DZCB(K))
        ENDDO

      ENDDO
      RETURN
      END


      REAL*8 FUNCTION TSTEP(F,FDOT,F2DOT,F3DOT,ETA)
*
*
*       General time-step criterion.
*       ----------------------------
*
      IMPLICIT REAL*8  (A-H,O-Z)
      REAL*8  F(3),FDOT(3),F2DOT(3),F3DOT(3)
*
*       Obtain new integration step using composite expression.
*       STEP = (ETA*(F*F2DOT + FDOT**2)/(FDOT*F3DOT + F2DOT**2))**0.5.
*
      F2 = F(1)**2 + F(2)**2 + F(3)**2
      FDOT2 = FDOT(1)**2 + FDOT(2)**2 + FDOT(3)**2
      F2DOT2 = F2DOT(1)**2 + F2DOT(2)**2 + F2DOT(3)**2
      F3DOT2 = F3DOT(1)**2 + F3DOT(2)**2 + F3DOT(3)**2
*
      TSTEP = (SQRT(F2*F2DOT2) + FDOT2)/(SQRT(FDOT2*F3DOT2) + F2DOT2)
      TSTEP = SQRT(ETA*TSTEP)
*
      RETURN
*
      END


      subroutine strap (x, y, n, sum)

c     Trapeziodal integration of y(x), result in sum
c     SINGLE PRECISION
c     modified by ala to handle cases for E<Efermi
c     sum only positive numbers

      dimension x(n), y(n)

      sum = y(1) * abs(x(2) - x(1))
      do 10  i = 2, n-1
         sum = sum + y(i) * abs(x(i+1) - x(i-1))
   10 continue
      sum = sum + y(n) * abs(x(n) - x(n-1))
      sum = sum/2

      return
      end


module laplace_assembly
use types, only: dp
implicit none
private
public assemble_2d

real(dp), parameter :: omega = 1.138_dp

contains

real(dp) elemental function f(x, y)
real(dp), intent(in) :: x, y
f = omega**2 * (x**2 + y**2) / 2  ! Harmonic oscillator
end function

subroutine assemble_2d(xin, nodes, elems, ib, xiq, wtq, phihq, dphihq, Am, Bm)
! Assemble on a 2D rectangular uniform mesh
real(dp), intent(in):: xin(:), nodes(:, :), xiq(:), wtq(:, :), &
    phihq(:, :), dphihq(:, :)
integer, intent(in):: elems(:, :), ib(:, :, :)
real(dp), intent(out):: Am(:,:), Bm(:, :)
integer :: Ne, p, e, i, j, iqx, iqy
real(dp), dimension(size(xiq), size(xiq), size(xin), size(xin)) :: &
    phi_v, phi_dx, phi_dy
real(dp) :: x(size(xiq)), y(size(xiq))
real(dp) :: fq(size(xiq), size(xiq)), xp(size(xiq)), yp(size(xiq))
real(dp) :: lx, ly
integer :: ax, ay, bx, by
real(dp) :: jacx, jacy, jac_det

Ne = size(elems, 2)
p = size(xin) - 1
! 2D shape functions
do ay = 1, p+1
    do ax = 1, p+1
        do iqy = 1, size(xiq)
            do iqx = 1, size(xiq)
                phi_v(iqx, iqy, ax, ay) = phihq(iqx, ax)*phihq(iqy, ay)
                phi_dx(iqx, iqy, ax, ay) = dphihq(iqx, ax)*phihq(iqy, ay)
                phi_dy(iqx, iqy, ax, ay) = phihq(iqx, ax)*dphihq(iqy, ay)
            end do
        end do
   end do
end do
Am=0; Bm=0
! Precalculate as much as possible:
lx = nodes(1, elems(3, 1)) - nodes(1, elems(1, 1)) ! Element sizes
ly = nodes(2, elems(3, 1)) - nodes(2, elems(1, 1))
jacx = lx/2
jacy = ly/2
jac_det = abs(jacx*jacy)
xp = (xiq + 1) * jacx
yp = (xiq + 1) * jacy
phi_dx = phi_dx / jacx
phi_dy = phi_dy / jacy
do e = 1, Ne
    x = xp + nodes(1, elems(1, e))
    y = yp + nodes(2, elems(1, e))
    do iqy = 1, size(xiq)
    do iqx = 1, size(xiq)
        fq(iqx, iqy) = f(x(iqx), y(iqy))
    end do
    end do
    fq = fq * jac_det * wtq
    do by = 1, p+1
    do bx = 1, p+1
        j = ib(bx, by, e)
        if (j==0) cycle
        do ay = 1, p+1
        do ax = 1, p+1
            i = ib(ax, ay, e)
            if (i == 0) cycle
            if (j > i) cycle
            Am(i,j) = Am(i,j) + sum( &
                    (phi_dx(:, :, ax, ay)*phi_dx(:, :, bx, by) &
                    +phi_dy(:, :, ax, ay)*phi_dy(:, :, bx, by)) &
                * jac_det * wtq) / 2
            Am(i,j) = Am(i,j) + sum(fq * &
                phi_v(:, :, ax, ay)*phi_v(:, :, bx, by) &
                * jac_det * wtq)
            Bm(i,j) = Bm(i,j) + sum((phi_v(:, :, ax, ay)*phi_v(:, :, bx, by) &
                * jac_det * wtq))
        end do
        end do
    end do
    end do
end do
do j = 1, size(Am, 2)
    do i = 1, j-1
        Am(i, j) = Am(j, i)
        Bm(i, j) = Bm(j, i)
    end do
end do
end subroutine

end module

program laplace2d_eig

use types, only: dp
use fe_mesh, only: cartesian_mesh_2d, cartesian_mesh_3d, &
    define_connect_tensor_2d, c2fullc_2d, fe2quad_2d
use laplace_assembly, only: assemble_2d
use feutils, only: get_parent_nodes, get_parent_quad_pts_wts, phih, dphih
use linalg, only: eigh
use constants, only: pi
implicit none

integer :: Nn, Ne
! nodes(:, i) are the (x,y) coordinates of the i-th mesh node
real(dp), allocatable :: nodes(:, :)
integer, allocatable :: elems(:, :) ! elems(:, i) are nodes of the i-th element
integer :: Nq, p, Nb
real(dp), allocatable :: xin(:), xiq(:), wtq(:), A(:, :), B(:, :), c(:, :), &
    lam(:), wtq2(:, :), phihq(:, :), dphihq(:, :)
integer, allocatable :: ib(:, :, :), in(:, :, :)
real(dp) :: rmax
integer :: i, j, Nex, Ney

Nex = 1
Ney = 1
p = 3
Nq = 4
rmax = pi/2

call cartesian_mesh_2d(Nex, Ney, [-rmax, -rmax], [rmax, rmax], nodes, elems)
Nn = size(nodes, 2)
Ne = size(elems, 2)

print *, "Number of nodes:", Nn
print *, "Number of elements:", Ne

allocate(xin(p+1))
call get_parent_nodes(2, p, xin)
allocate(xiq(Nq), wtq(Nq), wtq2(Nq, Nq))
call get_parent_quad_pts_wts(1, Nq, xiq, wtq)
forall(i=1:Nq, j=1:Nq) wtq2(i, j) = wtq(i)*wtq(j)
allocate(phihq(size(xiq), size(xin)))
allocate(dphihq(size(xiq), size(xin)))
! Tabulate parent basis at quadrature points
forall(i=1:size(xiq), j=1:size(xin))  phihq(i, j) =  phih(xin, j, xiq(i))
forall(i=1:size(xiq), j=1:size(xin)) dphihq(i, j) = dphih(xin, j, xiq(i))

call define_connect_tensor_2d(Nex, Ney, p, 1, in)
call define_connect_tensor_2d(Nex, Ney, p, 2, ib)
Nb = maxval(ib)
print *, "p =", p
print *, "DOFs =", Nb
allocate(A(Nb, Nb), B(Nb, Nb), c(Nb, Nb), lam(Nb))

call assemble_2d(xin, nodes, elems, ib, xiq, wtq2, phihq, dphihq, A, B)
call eigh(A, B, lam, c)
print *, "Eigenvalues:"
do i = 1, min(Nb, 20)
    print *, i, lam(i)
end do
end program