! =============================================================================
!                        Test parcel correction module
!
!         This unit test checks the parcel correction by initializing
!         the parcels with a small deviation from the optimal position.
!         It then performs 20 relaxation steps.
! =============================================================================
program test_parcel_correction
    use unit_test
    use options, only : parcel
    use constants, only : pi, one, zero, two, f14, f32
    use parcel_container
    use parcel_bc, only : apply_periodic_bc, apply_reflective_bc
    use parcel_correction
    use parcel_interpl, only : vol2grid
    use parcel_ellipse, only : get_ab
    use parcel_init, only : init_regular_positions
    use parameters, only : lower, extent, update_parameters, vcell, nx, nz, dx
    use fields, only : volg
    use timer
    implicit none

    double precision :: final_error, init_error, val, tmp
    integer :: i, n, sk
    integer, allocatable :: seed(:)
    double precision :: q, m, delta

    call random_seed(size=sk)
    allocate(seed(1:sk))
    seed(:) = 42
    call random_seed(put=seed)

    call  parse_command_line

    call register_timer('laplace correction', lapl_corr_timer)
    call register_timer('gradient correction', grad_corr_timer)

    nx = 32
    nz = 32
    lower  = (/zero, zero/)
    extent = (/one, one/)

    call update_parameters

    allocate(volg(-1:nz+1, 0:nx-1))

    n_parcels = 4*nx*nz
    call parcel_alloc(n_parcels)

    parcel%n_per_cell = 4
    call init_regular_positions

    volg = zero

    parcels%volume = f14 * vcell

    ! b11
    parcels%B(1, :) = get_ab(parcels%volume(1:n_parcels))

    ! b12
    parcels%B(2, :) = zero

    ! 0 --> -delta
    ! 1 -->  delta
    ! y = mx + q
    ! -delta = q
    ! delta = m - delta --> m = 2 * delta

    delta = f14 * dx(1)

    q = - delta
    m = two * delta

    ! add some deviation
    do n = 1, n_parcels
        call random_number(val)

        tmp = m * val + q
        parcels%position(1, n) = parcels%position(1, n) + tmp

        call random_number(val)
        tmp = m * val + q
        parcels%position(2, n) = parcels%position(2, n) + tmp

        call apply_periodic_bc(parcels%position(:, n))
        call apply_reflective_bc(parcels%position(:, n), parcels%B(:, n))
    enddo

    call vol2grid

    init_error = sum(abs(volg(0:nz, 0:nx-1) / vcell - one)) / (nx * (nz+1))

    if (verbose) then
        open(unit=12, file='test_parcel_correction.asc', status='replace')
        write(12,*) '# iteration, average error, max absolute error'
        write(12,*) 0, init_error, maxval(abs(volg(0:nz, 0:nx-1) / vcell - one))
    endif

    if (verbose) then
        open(unit=1235, file='initial_parcel_corr.asc', status='replace')
        write(1235, *) '# x, y volume B11, B22'
        do n = 1, n_parcels
            write(1235, *) parcels%position(:, n), parcels%volume(n), parcels%B(:, n)
        enddo
        close(1235)
    endif

    call init_parcel_correction

    do i = 1, 20
        call apply_laplace
        call vol2grid
        call apply_gradient(1.80d0,0.5d0)
        if (verbose) then
            call vol2grid
            write(12, *) i, sum(abs(volg(0:nz, 0:nx-1) / vcell - one)) / (nx * (nz+1)), &
                            maxval(abs(volg(0:nz, 0:nx-1) / vcell - one))
        endif

        if (verbose .and. (i == 2)) then
            open(unit=1236, file='two_iter_parcel_corr.asc', status='replace')
            write(1236, *) '# x, y volume B11, B22'
            do n = 1, n_parcels
                write(1236, *) parcels%position(:, n), parcels%volume(n), parcels%B(:, n)
            enddo
            close(1236)
        endif

    enddo

    if (verbose) then
        close(12)
    endif

    call vol2grid

    final_error = sum(abs(volg(0:nz, 0:nx-1) / vcell - one)) / (nx * (nz+1))

    call print_result_dp('Test laplace and gradient', final_error, init_error)

    deallocate(volg)

end program test_parcel_correction


      FUNCTION erfc(x)
      REAL erfc,x
CU    USES gammp,gammq
      REAL gammp,gammq
      if(x.lt.0.)then
        erfc=1.+gammp(.5,x**2)
      else
        erfc=gammq(.5,x**2)
      endif
      return
      END

NR-Functions/Numerical Recipes- Example & Functions/Functions/erfc.for


subroutine symmetric_jaccard_kernel(n_items, vec1, kernel)

    ! lengths = np.sum(vectors, axis=1)
    ! shape = lengths.shape[0]
    ! lengths = lengths.reshape((shape, 1))
    !
    ! kernel = np.dot(vectors, vectors.T)
    ! kernel = kernel / (lengths + lengths.T - kernel)

    implicit none

    integer, intent(in) :: n_items
    integer, intent(in), dimension(:,:) :: vec1
    double precision, intent(out), dimension(n_items,n_items) :: kernel
    integer :: i, j
    double precision :: dp
    integer, dimension(:), allocatable :: lengths

    allocate(lengths(n_items))

    !$omp parallel do
    do i = 1, n_items
        lengths(i) = sum(vec1(:,i))
    end do
    !$omp end parallel do

    !$omp parallel do private(dp) schedule(dynamic)
    do i = 1, n_items
        do j = i, n_items

            dp = DOT_PRODUCT(vec1(:,i), vec1(:,j))

            kernel(i,j) = dp / (lengths(i) + lengths(j) - dp)
            kernel(j,i) = kernel(i,j)

        end do
    end do
    !$omp end parallel do

    deallocate(lengths)

end subroutine symmetric_jaccard_kernel


CCC
CCC from http://cernlib.web.cern.ch/cernlib/download/2005_source/src/mathlib/gen/v/rm48.F
CCC
*
*
* Revision 1.2  2008/04/09 14:11:58  marafino
* Insert new and changed files to accomodate running this code as a producer
* or as a source.
*
* Revision 1.1  2007/02/06 14:40:25  eperez
* first version
*
* Revision 1.2  1996/12/12 16:32:06  cernlib
* Variables ONE and ZERO added to SAVE statement, courtesy R.Veenhof
*
* Revision 1.1.1.1  1996/04/01 15:02:55  mclareni
* Mathlib gen
*
*
      SUBROUTINE RM48(RVEC,LENV)
C     Double-precision version of
C Universal random number generator proposed by Marsaglia and Zaman
C in report FSU-SCRI-87-50
C        based on RANMAR, modified by F. James, to generate vectors
C        of pseudorandom numbers RVEC of length LENV, where the numbers
C        in RVEC are numbers with at least 48-bit mantissas.
C   Input and output entry points: RM48IN, RM48UT.
C!!! ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
C!!!  Calling sequences for RM48:                                    ++
C!!!      CALL RM48 (RVEC, LEN)     returns a vector RVEC of LEN     ++
C!!!                   64-bit random floating point numbers between  ++
C!!!                   zero and one.                                 ++
C!!!      CALL RM48IN(I1,N1,N2)   initializes the generator from one ++
C!!!                   64-bit integer I1, and number counts N1,N2    ++
C!!!                  (for initializing, set N1=N2=0, but to restart ++
C!!!                    a previously generated sequence, use values  ++
C!!!                    output by RM48UT)                            ++
C!!!      CALL RM48UT(I1,N1,N2)   outputs the value of the original  ++
C!!!                  seed and the two number counts, to be used     ++
C!!!                  for restarting by initializing to I1 and       ++
C!!!                  skipping N2*100000000+N1 numbers.              ++
C!!! ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
C for 32-bit machines, use IMPLICIT DOUBLE PRECISION
      IMPLICIT DOUBLE PRECISION (A-H,O-Z)
      DIMENSION RVEC(*)
      COMMON/R48ST1/U(97),C,I97,J97
      PARAMETER (MODCNS=1000000000)
      SAVE CD, CM, TWOM24, NTOT, NTOT2, IJKL,TWOM49, ONE, ZERO
      DATA NTOT,NTOT2,IJKL/-1,0,0/
C
      IF (NTOT .GE. 0)  GO TO 50
C
C        Default initialization. User has called RM48 without RM48IN.
      IJKL = 54217137
      NTOT = 0
      NTOT2 = 0
      KALLED = 0
      GO TO 1
C
      ENTRY      RM48IN(IJKLIN, NTOTIN,NTOT2N)
C         Initializing routine for RM48, may be called before
C         generating pseudorandom numbers with RM48.   The input
C         values should be in the ranges:  0<=IJKLIN<=900 OOO OOO
C                                          0<=NTOTIN<=999 999 999
C                                          0<=NTOT2N<<999 999 999!
C To get the standard values in Marsaglia's paper, IJKLIN=54217137
C                                            NTOTIN,NTOT2N=0
      IJKL = IJKLIN
      NTOT = MAX(NTOTIN,0)
      NTOT2= MAX(NTOT2N,0)
      KALLED = 1
C          always come here to initialize
    1 CONTINUE
      IJ = IJKL/30082
      KL = IJKL - 30082*IJ
      I = MOD(IJ/177, 177) + 2
      J = MOD(IJ, 177)     + 2
      K = MOD(KL/169, 178) + 1
      L = MOD(KL, 169)
      WRITE(6,'(A,I10,2X,2I10)') ' RM48 INITIALIZED:',IJKL,NTOT,NTOT2
CCC      PRINT '(A,4I10)', '   I,J,K,L= ',I,J,K,L
      ONE = 1.
      HALF = 0.5
      ZERO = 0.
      DO 2 II= 1, 97
      S = 0.
      T = HALF
      DO 3 JJ= 1, 48
         M = MOD(MOD(I*J,179)*K, 179)
         I = J
         J = K
         K = M
         L = MOD(53*L+1, 169)
         IF (MOD(L*M,64) .GE. 32)  S = S+T
    3    T = HALF*T
    2 U(II) = S
      TWOM49 = T
      TWOM24 = ONE
      DO 4 I24= 1, 24
    4 TWOM24 = HALF*TWOM24
      C  =   362436.*TWOM24
      CD =  7654321.*TWOM24
      CM = 16777213.*TWOM24
      I97 = 97
      J97 = 33
C       Complete initialization by skipping
C            (NTOT2*MODCNS + NTOT) random numbers
      DO 45 LOOP2= 1, NTOT2+1
      NOW = MODCNS
      IF (LOOP2 .EQ. NTOT2+1)  NOW=NTOT
      IF (NOW .GT. 0)  THEN
      WRITE(6,'(A,I15)') ' RM48IN SKIPPING OVER ',NOW
          DO 40 IDUM = 1, NTOT
          UNI = U(I97)-U(J97)
          IF (UNI .LT. ZERO)  UNI=UNI+ONE
          U(I97) = UNI
          I97 = I97-1
          IF (I97 .EQ. 0)  I97=97
          J97 = J97-1
          IF (J97 .EQ. 0)  J97=97
          C = C - CD
          IF (C .LT. ZERO)  C=C+CM
   40     CONTINUE
      ENDIF
   45 CONTINUE
      IF (KALLED .EQ. 1)  RETURN
C
C          Normal entry to generate LENV random numbers
C          Modified 13 Marcch '08 to use the CLHEP engines
   50 CONTINUE
      DO 100 IVEC= 1, LENV
*     UNI = U(I97)-U(J97)
*     IF (UNI .LT. ZERO)  UNI=UNI+ONE
*     U(I97) = UNI
*     I97 = I97-1
*     IF (I97 .EQ. 0)  I97=97
*     J97 = J97-1
*     IF (J97 .EQ. 0)  J97=97
*     C = C - CD
*     IF (C .LT. ZERO)  C=C+CM
*     UNI = UNI-C
*     IF (UNI .LT. ZERO) UNI=UNI+ONE
*     RVEC(IVEC) = UNI
      UNI = BHGPYR(IDUMMY)
      RVEC(IVEC) = UNI
C             Replace exact zeros by 2**-49
*        IF (UNI .EQ. ZERO)  THEN
*           RVEC(IVEC) = TWOM49
*        ENDIF
  100 CONTINUE
      NTOT = NTOT + LENV
         IF (NTOT .GE. MODCNS)  THEN
         NTOT2 = NTOT2 + 1
         NTOT = NTOT - MODCNS
         ENDIF
      RETURN
C           Entry to output current status
      ENTRY RM48UT(IJKLUT,NTOTUT,NTOT2T)
      IJKLUT = IJKL
      NTOTUT = NTOT
      NTOT2T = NTOT2
      RETURN
      END


module model_entropy
#include <messenger.h>
    use mod_kinds,          only: rk
    use mod_constants,      only: HALF, ONE, ZERO
    use mod_fluid,          only: Rgas, gam
    use type_model,         only: model_t
    use type_chidg_worker,  only: chidg_worker_t
    use DNAD_D
    implicit none

    !>  An equation of state model for an ideal gas.
    !!
    !!  Model Fields:
    !!      - Pressure
    !!      - Temperature
    !!
    !!  @author Nathan A. Wukie
    !!  @date   12/1/2016
    !!
    !---------------------------------------------------------------------------------------
    type, extends(model_t)  :: entropy_t

    contains

        procedure   :: init
        procedure   :: compute

    end type entropy_t
    !***************************************************************************************

contains

    !>  Initialize the model with a name and the model fields it is contributing to.
    !!
    !!  @author Nathan A. Wukie
    !!  @date   12/1/2016
    !!
    !---------------------------------------------------------------------------------------
    subroutine init(self)
        class(entropy_t), intent(inout)   :: self

        call self%set_name('Entropy')
        call self%set_dependency('f(Q-)')

        call self%add_model_field('Entropy')

    end subroutine init
    !***************************************************************************************

    !>  Routine for computing the Pressure and Temperature.
    !!
    !!  @author Nathan A. Wukie
    !!  @date   12/1/2016
    !!
    !--------------------------------------------------------------------------------------
    subroutine compute(self,worker)
        class(entropy_t),     intent(in)      :: self
        type(chidg_worker_t),   intent(inout)   :: worker

        type(AD_D), dimension(:),   allocatable :: &
            density, pressure, entropy

        !
        ! Interpolate solution to quadrature nodes
        !
        density = worker%get_field('Density',    'value')
        pressure  = worker%get_field('Pressure',     'value')

        entropy = pressure/(density**gam)

        call worker%store_model_field('Entropy',    'value', entropy)

    end subroutine compute
    !***************************************************************************************

end module model_entropy


!*==dggglm.f90  processed by SPAG 7.51RB at 20:08 on  3 Mar 2022
!> \brief \b DGGGLM
!
!  =========== DOCUMENTATION ===========
!
! Online html documentation available at
!            http://www.netlib.org/lapack/explore-html/
!
!> \htmlonly
!> Download DGGGLM + dependencies
!>
!> [TGZ]
!>
!> [ZIP]
!>
!> [TXT]
!> \endhtmlonly
!
!  Definition:
!  ===========
!
!       SUBROUTINE DGGGLM( N, M, P, A, LDA, B, LDB, D, X, Y, WORK, LWORK,
!                          INFO )
!
!       .. Scalar Arguments ..
!       INTEGER            INFO, LDA, LDB, LWORK, M, N, P
!       ..
!       .. Array Arguments ..
!       DOUBLE PRECISION   A( LDA, * ), B( LDB, * ), D( * ), WORK( * ),
!      $                   X( * ), Y( * )
!       ..
!
!
!> \par Purpose:
!  =============
!>
!> \verbatim
!>
!> DGGGLM solves a general Gauss-Markov linear model (GLM) problem:
!>
!>         minimize || y ||_2   subject to   d = A*x + B*y
!>             x
!>
!> where A is an N-by-M matrix, B is an N-by-P matrix, and d is a
!> given N-vector. It is assumed that M <= N <= M+P, and
!>
!>            rank(A) = M    and    rank( A B ) = N.
!>
!> Under these assumptions, the constrained equation is always
!> consistent, and there is a unique solution x and a minimal 2-norm
!> solution y, which is obtained using a generalized QR factorization
!> of the matrices (A, B) given by
!>
!>    A = Q*(R),   B = Q*T*Z.
!>          (0)
!>
!> In particular, if matrix B is square nonsingular, then the problem
!> GLM is equivalent to the following weighted linear least squares
!> problem
!>
!>              minimize || inv(B)*(d-A*x) ||_2
!>                  x
!>
!> where inv(B) denotes the inverse of B.
!> \endverbatim
!
!  Arguments:
!  ==========
!
!> \param[in] N
!> \verbatim
!>          N is INTEGER
!>          The number of rows of the matrices A and B.  N >= 0.
!> \endverbatim
!>
!> \param[in] M
!> \verbatim
!>          M is INTEGER
!>          The number of columns of the matrix A.  0 <= M <= N.
!> \endverbatim
!>
!> \param[in] P
!> \verbatim
!>          P is INTEGER
!>          The number of columns of the matrix B.  P >= N-M.
!> \endverbatim
!>
!> \param[in,out] A
!> \verbatim
!>          A is DOUBLE PRECISION array, dimension (LDA,M)
!>          On entry, the N-by-M matrix A.
!>          On exit, the upper triangular part of the array A contains
!>          the M-by-M upper triangular matrix R.
!> \endverbatim
!>
!> \param[in] LDA
!> \verbatim
!>          LDA is INTEGER
!>          The leading dimension of the array A. LDA >= max(1,N).
!> \endverbatim
!>
!> \param[in,out] B
!> \verbatim
!>          B is DOUBLE PRECISION array, dimension (LDB,P)
!>          On entry, the N-by-P matrix B.
!>          On exit, if N <= P, the upper triangle of the subarray
!>          B(1:N,P-N+1:P) contains the N-by-N upper triangular matrix T;
!>          if N > P, the elements on and above the (N-P)th subdiagonal
!>          contain the N-by-P upper trapezoidal matrix T.
!> \endverbatim
!>
!> \param[in] LDB
!> \verbatim
!>          LDB is INTEGER
!>          The leading dimension of the array B. LDB >= max(1,N).
!> \endverbatim
!>
!> \param[in,out] D
!> \verbatim
!>          D is DOUBLE PRECISION array, dimension (N)
!>          On entry, D is the left hand side of the GLM equation.
!>          On exit, D is destroyed.
!> \endverbatim
!>
!> \param[out] X
!> \verbatim
!>          X is DOUBLE PRECISION array, dimension (M)
!> \endverbatim
!>
!> \param[out] Y
!> \verbatim
!>          Y is DOUBLE PRECISION array, dimension (P)
!>
!>          On exit, X and Y are the solutions of the GLM problem.
!> \endverbatim
!>
!> \param[out] WORK
!> \verbatim
!>          WORK is DOUBLE PRECISION array, dimension (MAX(1,LWORK))
!>          On exit, if INFO = 0, WORK(1) returns the optimal LWORK.
!> \endverbatim
!>
!> \param[in] LWORK
!> \verbatim
!>          LWORK is INTEGER
!>          The dimension of the array WORK. LWORK >= max(1,N+M+P).
!>          For optimum performance, LWORK >= M+min(N,P)+max(N,P)*NB,
!>          where NB is an upper bound for the optimal blocksizes for
!>          DGEQRF, SGERQF, DORMQR and SORMRQ.
!>
!>          If LWORK = -1, then a workspace query is assumed; the routine
!>          only calculates the optimal size of the WORK array, returns
!>          this value as the first entry of the WORK array, and no error
!>          message related to LWORK is issued by XERBLA.
!> \endverbatim
!>
!> \param[out] INFO
!> \verbatim
!>          INFO is INTEGER
!>          = 0:  successful exit.
!>          < 0:  if INFO = -i, the i-th argument had an illegal value.
!>          = 1:  the upper triangular factor R associated with A in the
!>                generalized QR factorization of the pair (A, B) is
!>                singular, so that rank(A) < M; the least squares
!>                solution could not be computed.
!>          = 2:  the bottom (N-M) by (N-M) part of the upper trapezoidal
!>                factor T associated with B in the generalized QR
!>                factorization of the pair (A, B) is singular, so that
!>                rank( A B ) < N; the least squares solution could not
!>                be computed.
!> \endverbatim
!
!  Authors:
!  ========
!
!> \author Univ. of Tennessee
!> \author Univ. of California Berkeley
!> \author Univ. of Colorado Denver
!> \author NAG Ltd.
!
!> \date December 2016
!
!> \ingroup doubleOTHEReigen
!
!  =====================================================================
      SUBROUTINE DGGGLM(N,M,P,A,Lda,B,Ldb,D,X,Y,Work,Lwork,Info)
      IMPLICIT NONE
!*--DGGGLM188
!
!  -- LAPACK driver routine (version 3.7.0) --
!  -- LAPACK is a software package provided by Univ. of Tennessee,    --
!  -- Univ. of California Berkeley, Univ. of Colorado Denver and NAG Ltd..--
!     December 2016
!
!     .. Scalar Arguments ..
      INTEGER Info , Lda , Ldb , Lwork , M , N , P
!     ..
!     .. Array Arguments ..
      DOUBLE PRECISION A(Lda,*) , B(Ldb,*) , D(*) , Work(*) , X(*) ,    &
     &                 Y(*)
!     ..
!
!  ===================================================================
!
!     .. Parameters ..
      DOUBLE PRECISION ZERO , ONE
      PARAMETER (ZERO=0.0D+0,ONE=1.0D+0)
!     ..
!     .. Local Scalars ..
      LOGICAL lquery
      INTEGER i , lopt , lwkmin , lwkopt , nb , nb1 , nb2 , nb3 , nb4 , &
     &        np
!     ..
!     .. External Subroutines ..
      EXTERNAL DCOPY , DGEMV , DGGQRF , DORMQR , DORMRQ , DTRTRS ,      &
     &         XERBLA
!     ..
!     .. External Functions ..
      INTEGER ILAENV
      EXTERNAL ILAENV
!     ..
!     .. Intrinsic Functions ..
      INTRINSIC INT , MAX , MIN
!     ..
!     .. Executable Statements ..
!
!     Test the input parameters
!
      Info = 0
      np = MIN(N,P)
      lquery = (Lwork==-1)
      IF ( N<0 ) THEN
         Info = -1
      ELSEIF ( M<0 .OR. M>N ) THEN
         Info = -2
      ELSEIF ( P<0 .OR. P<N-M ) THEN
         Info = -3
      ELSEIF ( Lda<MAX(1,N) ) THEN
         Info = -5
      ELSEIF ( Ldb<MAX(1,N) ) THEN
         Info = -7
      ENDIF
!
!     Calculate workspace
!
      IF ( Info==0 ) THEN
         IF ( N==0 ) THEN
            lwkmin = 1
            lwkopt = 1
         ELSE
            nb1 = ILAENV(1,'DGEQRF',' ',N,M,-1,-1)
            nb2 = ILAENV(1,'DGERQF',' ',N,M,-1,-1)
            nb3 = ILAENV(1,'DORMQR',' ',N,M,P,-1)
            nb4 = ILAENV(1,'DORMRQ',' ',N,M,P,-1)
            nb = MAX(nb1,nb2,nb3,nb4)
            lwkmin = M + N + P
            lwkopt = M + np + MAX(N,P)*nb
         ENDIF
         Work(1) = lwkopt
!
         IF ( Lwork<lwkmin .AND. .NOT.lquery ) Info = -12
      ENDIF
!
      IF ( Info/=0 ) THEN
         CALL XERBLA('DGGGLM',-Info)
         RETURN
      ELSEIF ( lquery ) THEN
         RETURN
      ENDIF
!
!     Quick return if possible
!
      IF ( N==0 ) THEN
         DO i = 1 , M
            X(i) = ZERO
         ENDDO
         DO i = 1 , P
            Y(i) = ZERO
         ENDDO
         RETURN
      ENDIF
!
!     Compute the GQR factorization of matrices A and B:
!
!          Q**T*A = ( R11 ) M,    Q**T*B*Z**T = ( T11   T12 ) M
!                   (  0  ) N-M                 (  0    T22 ) N-M
!                      M                         M+P-N  N-M
!
!     where R11 and T22 are upper triangular, and Q and Z are
!     orthogonal.
!
      CALL DGGQRF(N,M,P,A,Lda,Work,B,Ldb,Work(M+1),Work(M+np+1),        &
     &            Lwork-M-np,Info)
      lopt = Work(M+np+1)
!
!     Update left-hand-side vector d = Q**T*d = ( d1 ) M
!                                               ( d2 ) N-M
!
      CALL DORMQR('Left','Transpose',N,1,M,A,Lda,Work,D,MAX(1,N),       &
     &            Work(M+np+1),Lwork-M-np,Info)
      lopt = MAX(lopt,INT(Work(M+np+1)))
!
!     Solve T22*y2 = d2 for y2
!
      IF ( N>M ) THEN
         CALL DTRTRS('Upper','No transpose','Non unit',N-M,1,           &
     &               B(M+1,M+P-N+1),Ldb,D(M+1),N-M,Info)
!
         IF ( Info>0 ) THEN
            Info = 1
            RETURN
         ENDIF
!
         CALL DCOPY(N-M,D(M+1),1,Y(M+P-N+1),1)
      ENDIF
!
!     Set y1 = 0
!
      DO i = 1 , M + P - N
         Y(i) = ZERO
      ENDDO
!
!     Update d1 = d1 - T12*y2
!
      CALL DGEMV('No transpose',M,N-M,-ONE,B(1,M+P-N+1),Ldb,Y(M+P-N+1), &
     &           1,ONE,D,1)
!
!     Solve triangular system: R11*x = d1
!
      IF ( M>0 ) THEN
         CALL DTRTRS('Upper','No Transpose','Non unit',M,1,A,Lda,D,M,   &
     &               Info)
!
         IF ( Info>0 ) THEN
            Info = 2
            RETURN
         ENDIF
!
!        Copy D to X
!
         CALL DCOPY(M,D,1,X,1)
      ENDIF
!
!     Backward transformation y = Z**T *y
!
      CALL DORMRQ('Left','Transpose',P,1,np,B(MAX(1,N-P+1),1),Ldb,      &
     &            Work(M+1),Y,MAX(1,P),Work(M+np+1),Lwork-M-np,Info)
      Work(1) = M + np + MAX(lopt,INT(Work(M+np+1)))
!
!
!     End of DGGGLM
!
      END SUBROUTINE DGGGLM


 subroutine chobk2(kb,loads)
!Choleski back-substitution
 implicit none
 INTEGER,PARAMETER::iwp=SELECTED_REAL_KIND(15)
 REAL(iwp),intent(in)::kb(:,:)
 REAL(iwp),intent(in out)::loads(0:)
 integer::iw,n,i,j,l,m
 REAL(iwp)::x
 n=size(kb,1)
 iw=size(kb,2)-1
 loads(n)=loads(n)/kb(n,iw+1)
 do i=n-1,1,-1
   x=0.0
   l=i+iw
   if(i>n-iw)l=n
   m=i+1
   do j=m,l
     x=x+kb(j,iw+i-j+1)*loads(j)
   end do
   loads(i)=(loads(i)-x)/kb(i,iw+1)
 end do
 return
 end subroutine chobk2


      subroutine compute_sat_res_m(rp_div,rl_div,
     &rpix,rline,start_pix,start_line,instr,r_res_m,
     &istatus)
c
c
      include 'trigd.inc'
      implicit none

      Real*8      rlat8_1,rlat8_2,rlat8_3
      Real*8      rlon8_1,rlon8_2,rlon8_3
      Real*8      elev1,elev2,scan1,scan2
      Real*8      scpx,evln
      Real*8      RL
      Real*8      RP

      Real        rlat1,rlat2,rlat3,rlon1,rlon2,rlon3
      Real        grid_spacing_scan,grid_spacing_elev
      Real        rpix,rline
      Real*8      radtodeg
      Real*8      pi
      Real        r_res_m
c     Real        resscln
c     Real        reselev
      Real*8      rl_div
      Real*8      rp_div

      Integer     instr
      Integer     istatus
      Integer     ierr
      Integer     start_line
      Integer     start_pix
c --------------------------------------------
c start
c
c   x lat3,lon3
c
c   x lat1,lon1      x  lat2,lon2
c
      istatus=0
      pi=acos(-1.)
      radtodeg=180.d0/pi
c     instr = 1  !imager=1, sounder = 2.
c pix/line absolute coordinates
c
      RP= rpix*rp_div +float(start_pix)-rp_div
      RL= rline*rl_div+float(start_line)-rl_div
      elev1 = evln(instr,RL)
      scan1 = scpx(instr,RP)
      call lpoint(elev1,scan1,rlat8_1,rlon8_1,ierr)
c
      scan2 = scpx(instr,RP+rp_div)  !e-w increment
      elev2 = evln(instr,RL+rl_div)  !n-s increment
      call lpoint(elev1,scan2,rlat8_2,rlon8_2,ierr)
      call lpoint(elev2,scan1,rlat8_3,rlon8_3,ierr)

      rlat1=rlat8_1*radtodeg
      rlat2=rlat8_2*radtodeg
      rlon1=rlon8_1*radtodeg
      rlon2=rlon8_2*radtodeg
      rlat3=rlat8_3*radtodeg
      rlon3=rlon8_3*radtodeg

c     r_res_m=sqrt((rlat3-rlat1)**2+
c    1            ((rlon2-rlon1)*cosd(rlat1))**2)
c     r_res_m=r_res_m*111.1*1000.

c     resscln=abs(rlon2-rlon1)*111.1*1000.
c     reselev=abs(rlat1-rlat3)*111.1*1000.
c     r_res_m=(resscln+reselev)/2.0                        !result in meters

      grid_spacing_scan = sqrt( (rlat1-rlat2)**2 +
     1             ((rlon1-rlon2)*cosd(rlat1))**2)
      grid_spacing_scan = grid_spacing_scan*111.1*1000.0   !result in meters

      grid_spacing_elev=sqrt( (rlat1-rlat3)**2 +
     1             ((rlon1-rlon3)*cosd(rlat1))**2)
      grid_spacing_elev=grid_spacing_elev*111.1*1000.0   !result in meters

      r_res_m=(grid_spacing_scan+grid_spacing_elev)/2.0

      write(6,*)'Image Resolution (m): ',r_res_m

      if(ierr.ne.0)istatus=-1
      return
      end


module CubicGLLModule

use NumberKindsModule
use UtilitiesModule

implicit none

public

integer(kint), parameter :: MAX_ITER = 20 !< maximum iterations for Newton's method (used in inverseSphereMap)

real(kreal), parameter :: r5 = sqrt(5.0_kreal) !< timesaver
real(kreal), parameter :: oor5 = 1.0_kreal / r5 !< timesaver

real(kreal), dimension(4), parameter :: gll_cubic_qp = (/ -1.0_kreal, -oor5, oor5, 1.0_kreal /) !< quadrature pts for cubic GLL
real(kreal), dimension(4), parameter :: gll_cubic_qw = (/ 1.0_kreal, 5.0_kreal, 5.0_kreal, 1.0_kreal /) / 6.0_kreal !< quadrature weights for cubic GLL

!> quadrature points re-indexed to match layout of quad16 faces, edges, and particles
real(kreal), dimension(2,12), parameter :: quad16_vertex_qp = reshape([ gll_cubic_qp(1), gll_cubic_qp(4), &
                                                                gll_cubic_qp(1), gll_cubic_qp(3), &
                                                                gll_cubic_qp(1), gll_cubic_qp(2), &
                                                                gll_cubic_qp(1), gll_cubic_qp(1), &
                                                                gll_cubic_qp(2), gll_cubic_qp(1), &
                                                                gll_cubic_qp(3), gll_cubic_qp(1), &
                                                                gll_cubic_qp(4), gll_cubic_qp(1), &
                                                                gll_cubic_qp(4), gll_cubic_qp(2), &
                                                                gll_cubic_qp(4), gll_cubic_qp(3), &
                                                                gll_cubic_qp(4), gll_cubic_qp(4), &
                                                                gll_cubic_qp(3), gll_cubic_qp(4), &
                                                                gll_cubic_qp(2), gll_cubic_qp(4)], &
                                                                 shape(quad16_vertex_qp))

!> quadrature points re-indexed to match layout of quad16 faces, edges, and particles
real(kreal), dimension(2,4), parameter :: quad16_center_qp = reshape([ gll_cubic_qp(2), gll_cubic_qp(3), &
                                                                gll_cubic_qp(2), gll_cubic_qp(2), &
                                                                gll_cubic_qp(3), gll_cubic_qp(2), &
                                                                gll_cubic_qp(3), gll_cubic_qp(3)], &
                                                                shape(quad16_center_qp))
!> quadrature weights re-indexed to match layout of quad16 faces, edges, and particles
real(kreal), dimension(12), parameter :: quad16_vertex_qw =(/gll_cubic_qw(1)*gll_cubic_qw(4), &
                                              gll_cubic_qw(1)*gll_cubic_qw(3), &
                                              gll_cubic_qw(1)*gll_cubic_qw(2), &
                                              gll_cubic_qw(1)*gll_cubic_qw(1), &
                                              gll_cubic_qw(2)*gll_cubic_qw(1), &
                                              gll_cubic_qw(3)*gll_cubic_qw(1), &
                                              gll_cubic_qw(4)*gll_cubic_qw(1), &
                                              gll_cubic_qw(4)*gll_cubic_qw(2), &
                                              gll_cubic_qw(4)*gll_cubic_qw(3), &
                                              gll_cubic_qw(4)*gll_cubic_qw(4), &
                                              gll_cubic_qw(3)*gll_cubic_qw(4), &
                                              gll_cubic_qw(2)*gll_cubic_qw(4) /)
!> quadrature weights re-indexed to match layout of quad16 faces, edges, and particles
real(kreal), dimension(4), parameter :: quad16_center_qw =(/gll_cubic_qw(2)*gll_cubic_qw(3), &
                                              gll_cubic_qw(2)*gll_cubic_qw(2), &
                                              gll_cubic_qw(3)*gll_cubic_qw(2), &
                                              gll_cubic_qw(3)*gll_cubic_qw(3) /)

contains

!> Computes the matrix associated with bilinearMap
pure function bilinearTransformMatrix(vertXyz)
    real(kreal), dimension(3,4) :: bilinearTransformMatrix
    real(kreal), dimension(3,4), intent(in) :: vertXyz
    real(kreal), dimension(4,4), parameter :: P = reshape([one, -one, one, -one, & !col 1
                                                  -one, one, dzero, dzero, & ! col2
                                                  -one, dzero, dzero, one, & ! col3
                                                  one, dzero, dzero, dzero], shape(P)) ! col4
    bilinearTransformMatrix = matmul(vertXyz, P)
end function

!> @brief Finds the point X inside a bilinear quadrilateral,
!> Given the vertices of a quadrilateral in R^d (d=2,3), and reference coordinates (r1,r2) in [-1,1]x[-1,1],
!>
!> @param vertXyz Vertex coordinates of quadrilateral, in CCW order
!> @param s1, s2 reference element coordinates in [-1,1]x[-1,1]
pure function bilinearMap(vertXyz, s1, s2)
    real(kreal), dimension(3) :: bilinearMap
    real(kreal), dimension(3,4), intent(in) :: vertXyz
    real(kreal), intent(in) :: s1, s2
    !
    real(kreal) :: a, b
    real(kreal), dimension(3,4) :: tMatrix
    integer(kint) :: i

    bilinearMap = 0.25_kreal * ( (1.0_kreal-s1)*(1.0_kreal+s2)*vertXyz(:,1) + &
                                 (1.0_kreal-s1)*(1.0_kreal-s2)*vertXyz(:,2) + &
                                (1.0_kreal+s1)*(1.0_kreal-s2)*vertXyz(:,3) + &
                                (1.0_kreal+s1)*(1.0_kreal+s2)*vertXyz(:,4))
end function

pure function bivariateScalarInterpolation(refCrd, vertVals, cntrVals)
    real(kreal) :: bivariateScalarInterpolation
    real(kreal), dimension(2), intent(in) :: refCrd
    real(kreal), dimension(12), intent(in) :: vertVals
    real(kreal), dimension(4), intent(in) :: cntrVals
    bivariateScalarInterpolation = dzero
    bivariateScalarInterpolation = bivariateScalarInterpolation + vertVals(1) * &
        gll_cubic_basis1(refCrd(1)) * gll_cubic_basis4(refCrd(2))
    bivariateScalarInterpolation = bivariateScalarInterpolation + vertVals(2) * &
        gll_cubic_basis1(refCrd(1)) * gll_cubic_basis3(refCrd(2))
    bivariateScalarInterpolation = bivariateScalarInterpolation + vertVals(3) * &
        gll_cubic_basis1(refCrd(1)) * gll_cubic_basis2(refCrd(2))
    bivariateScalarInterpolation = bivariateScalarInterpolation + vertVals(4) * &
        gll_cubic_basis1(refCrd(1)) * gll_cubic_basis1(refCrd(2))
    bivariateScalarInterpolation = bivariateScalarInterpolation + vertVals(5) * &
        gll_cubic_basis2(refCrd(1)) * gll_cubic_basis1(refCrd(2))
    bivariateScalarInterpolation = bivariateScalarInterpolation + vertVals(6) * &
        gll_cubic_basis3(refCrd(1)) * gll_cubic_basis1(refCrd(2))
    bivariateScalarInterpolation = bivariateScalarInterpolation + vertVals(7) * &
        gll_cubic_basis4(refCrd(1)) * gll_cubic_basis1(refCrd(2))
    bivariateScalarInterpolation = bivariateScalarInterpolation + vertVals(8) * &
        gll_cubic_basis4(refCrd(1)) * gll_cubic_basis2(refCrd(2))
    bivariateScalarInterpolation = bivariateScalarInterpolation + vertVals(9) * &
        gll_cubic_basis4(refCrd(1)) * gll_cubic_basis3(refCrd(2))
    bivariateScalarInterpolation = bivariateScalarInterpolation + vertVals(10) * &
        gll_cubic_basis4(refCrd(1)) * gll_cubic_basis4(refCrd(2))
    bivariateScalarInterpolation = bivariateScalarInterpolation + vertVals(11) * &
        gll_cubic_basis3(refCrd(1)) * gll_cubic_basis4(refCrd(2))
    bivariateScalarInterpolation = bivariateScalarInterpolation + vertVals(12) * &
        gll_cubic_basis2(refCrd(1)) * gll_cubic_basis4(refCrd(2))
    bivariateScalarInterpolation = bivariateScalarInterpolation + cntrVals(1) * &
        gll_cubic_basis2(refCrd(1)) * gll_cubic_basis3(refCrd(2))
    bivariateScalarInterpolation = bivariateScalarInterpolation + cntrVals(2) * &
        gll_cubic_basis2(refCrd(1)) * gll_cubic_basis2(refCrd(2))
    bivariateScalarInterpolation = bivariateScalarInterpolation + cntrVals(3) * &
        gll_cubic_basis3(refCrd(1)) * gll_cubic_basis2(refCrd(2))
    bivariateScalarInterpolation = bivariateScalarInterpolation + cntrVals(4) * &
        gll_cubic_basis3(refCrd(1)) * gll_cubic_basis3(refCrd(2))
end function

!> Inverts bilinearMap to find reference coordinates (r1,r2) in [-1,1]^2
!> Folows C. Hua, 1990, Finite Elem. Anal. Design 7:159--166.
function invertBilinear(vertXyz, xyz)
    real(kreal), dimension(2) :: invertBilinear
    real(kreal), dimension(3,4), intent(in) :: vertXyz
    real(kreal), dimension(3), intent(in) :: xyz
    !
    real(kreal), dimension(3) :: aa, bb, cc, dd
    integer(kint), dimension(2) :: ij
    real(kreal) :: xi, eta, a_b, a_c, a_d, b_c, b_d, c_d
    integer(kint) :: i, j

    aa = -vertXyz(:,1) + vertXyz(:,2) - vertXyz(:,3) + vertXyz(:,4)
    bb = -vertXyz(:,1) - vertXyz(:,2) + vertXyz(:,3) + vertXyz(:,4)
    cc = vertXyz(:,1) - vertXyz(:,2) - vertXyz(:,3) + vertXyz(:,4)

    dd = 4.0_kreal * xyz - sum(vertXyz,2)

    ij = pickBilinearIJ(vertXyz)
    i = ij(1)
    j = ij(2)

    xi = 999.0d20
    eta = 999.0d20

    a_b = twoByTwoDeterminant([aa(i),aa(j)],[bb(i),bb(j)])
    a_c = twoByTwoDeterminant([aa(i),aa(j)],[cc(i),cc(j)])
    a_d = twoByTwoDeterminant([aa(i),aa(j)],[dd(i),dd(j)])
    b_c = twoByTwoDeterminant([bb(i),bb(j)],[cc(i),cc(j)])
    b_d = twoByTwoDeterminant([bb(i),bb(j)],[dd(i),dd(j)])
    c_d = twoByTwoDeterminant([cc(i),cc(j)],[dd(i),dd(j)])

    if ( abs(aa(i)) < ZERO_TOL) then !I
        if (abs(aa(j)) < ZERO_TOL) then ! rectangle
            !I.A
            xi = -c_d / (aa(i)*dd(j) + b_c)
            eta = b_d /(aa(j)*dd(i) + b_c)
        elseif (abs(cc(i)) < ZERO_TOL) then
            !I.B(a)
            xi = dd(i) / bb(i)
            eta = (bb(i)*dd(j)-bb(j)*dd(i))/(aa(j)*dd(i)+bb(i)*cc(j))
        else
            !I.B(b)
            xi = quadraticRoot(aa(j)*bb(i), cc(j)*bb(i)-aa(j)*dd(i)-bb(j)*cc(i), dd(j)*cc(i)-cc(j)*dd(i))
            eta = (dd(i)-bb(i)*xi)/cc(i)
        endif
    else !II
        if (abs(aa(j)) > ZERO_TOL) then !II.A
            if (abs(a_b) > ZERO_TOL) then ! II.A(a)
                if (abs(a_c) > ZERO_TOL) then
                    !II.A.(a).1
                    xi = quadraticRoot(a_b, (-b_c-a_d), -c_d)
                    eta = (a_d-a_b*xi)/(a_c)
                else
                    !II.A.(a).2
                    xi = a_d/a_b
                    eta = -aa(i)*b_d/(cc(i)*a_b + aa(i)*a_d)
                endif
            else ! II.A(b)
                xi = -aa(i)*c_d/(bb(i)*a_c + aa(i)*a_d)
                eta = a_d/a_c
            endif
        else !II.B
            if (abs(bb(j)) < ZERO_TOL) then
                !II.B(a)
                xi = -c_d / (aa(i)*dd(j)+bb(i)*cc(j))
                eta = dd(j) / cc(j)
            else
                !II.B(b)
                xi = quadraticRoot(aa(i)*bb(j), (cc(i)*bb(j) - aa(i)*dd(j) - bb(i)*cc(j)), (dd(i)*cc(j)-cc(i)*dd(j)))
                eta = (dd(j)-bb(j)*xi)/cc(j)
            endif
        endif
    endif
    invertBilinear = [xi, eta]
end function

pure function twoByTwoDeterminant(c1, c2)
    real(kreal) :: twoByTwoDeterminant
    real(kreal), dimension(2), intent(in) :: c1, c2
    twoByTwoDeterminant = c1(1)*c2(2) - c1(2)*c2(1)
end function

function quadraticRoot(a, b, c)
    real(kreal) :: quadraticRoot
    real(kreal), intent(in) :: a, b, c
    !
    real(kreal) :: r1, r2, disc, aa, rdisc

    quadraticRoot = -999.d20

    aa = 2.0_kreal * a
    disc = b*b - 4.0_kreal * a * c
    if (abs(disc) < ZERO_TOL) then
        disc = dzero
    elseif (disc < -ZERO_TOL) then
        print *, "quadratic root error: complex root."
    endif
    rdisc = sqrt(disc)
    r1 = (-b + rdisc)/aa
    r2 = (-b - rdisc)/aa
    if (-one <= r1 .and. r1 <= one) quadraticRoot = r1
    if (-one <= r2 .and. r2 <= one) quadraticRoot = r2
end function

!> @brief Find the indices of the dimensions with the largest extent
!> for a bilinear quadrilateral in R^3; helps with stability in invertBilinear.
pure function pickBilinearIJ(vertXyz)
    integer(kint), dimension(2) :: pickBilinearIJ
    real(kreal), dimension(3,4), intent(in) :: vertXyz
    !
    real(kreal) :: dx, dy, dz

    dx = maxval(vertXyz(1,:)) - minval(vertXyz(1,:))
    dy = maxval(vertXyz(2,:)) - minval(vertXyz(2,:))
    dz = maxval(vertXyz(3,:)) - minval(vertXyz(3,:))

    ! start with x, y
    pickBilinearIJ(1) = 1
    pickBilinearIJ(2) = 2
    if (dy < dx .and. dy < dz) then ! skip y
        pickBilinearIJ(2) = 3
    elseif( dx < dy .and. dx < dz) then !skip x
        pickBilinearIJ(1) = 2
        pickBilinearIJ(2) = 3
    endif
end function

!> @brief Maps the reference element RQ = [-1,1]^2 to a spherical quadrilateral
function sphereQuadMap(vertXyz, s1, s2)
    real(kreal), dimension(3) :: sphereQuadMap
    real(kreal), dimension(3,4), intent(in) :: vertXyz
    real(kreal), intent(in) :: s1, s2
    !
    real(kreal) :: norm
    sphereQuadMap = bilinearMap(vertXyz, s1, s2)
    norm = sqrt(sum(vertXyz*vertXyz))
    sphereQuadMap = sphereQuadMap / norm
end function

!> @brief Computes the Jacobian matrix associated with sphereQuadMap
function sphereMapJacobian(xyz, vertXyz, s1, s2)
    real(kreal), dimension(3,3) :: sphereMapJacobian
    real(kreal), dimension(3), intent(in) :: xyz
    real(kreal), dimension(3,4), intent(in) :: vertXyz
    real(kreal), intent(in) :: s1, s2
    sphereMapJacobian = dzero
end function

!> @brief Computes the residual of sphereQuadMap; used for Newton iterations in invertSphereQuadMap.
pure function sphereMapResidual(xyz, vertXyz, s1, s2)
    real(kreal), dimension(3) :: sphereMapResidual
    real(kreal), dimension(3), intent(in) :: xyz
    real(kreal), dimension(3,4), intent(in) :: vertXyz
    real(kreal), intent(in) :: s1, s2
    !
    integer(kint) :: i
    real(kreal) :: norm
    real(kreal), dimension(3) :: blm

    blm = bilinearMap(vertXyz, s1, s2)
    norm = sqrt(sum(blm*blm))
    do i=1,3
        sphereMapResidual(i) = blm(i)/norm - xyz(i)
    enddo
end function

function invertSphereMap(xyz, vertXyz)
    real(kreal), dimension(2) :: invertSphereMap
    real(kreal), dimension(3) :: xyz
    real(kreal), dimension(3,4) :: vertXyz
    invertSphereMap = dzero
end function

!> @brief  Locates the interior points of a quadrilateral panel in R^2
pure function quad16InteriorPts(vertXYZ)
    real(kreal), dimension(3,4) :: quad16InteriorPts
    real(kreal), dimension(3,4), intent(in) :: vertXyz
    quad16InteriorPts(:,1) = bilinearMap(vertXyz, gll_cubic_qp(2), gll_cubic_qp(3))
    quad16InteriorPts(:,2) = bilinearMap(vertXyz, gll_cubic_qp(2), gll_cubic_qp(2))
    quad16InteriorPts(:,3) = bilinearMap(vertXyz, gll_cubic_qp(3), gll_cubic_qp(2))
    quad16InteriorPts(:,4) = bilinearMap(vertXyz, gll_cubic_qp(3), gll_cubic_qp(3))
end function

!> @brief Computes the Jacobian of the bilinearMap for planar panels.
pure function bilinearPlaneJacobian(vertXyz, s1, s2)
    real(kreal) :: bilinearPlaneJacobian
    real(kreal), dimension(3,4), intent(in) :: vertXyz
    real(kreal), intent(in) :: s1, s2
    !
    real(kreal) :: a, b, c, d

    a = 0.25_kreal * (-(1.0_kreal+s2)*vertXyz(1,1) -(1.0_kreal-s2)*vertXyz(1,2) +&
         (1.0_kreal-s2)*vertXyz(1,3) + (1.0_kreal+s2)*vertXyz(1,4))

    b = 0.25_kreal * (-(1.0_kreal+s2)*vertXyz(2,1) -(1.0_kreal-s2)*vertXyz(2,2) +&
         (1.0_kreal-s2)*vertXyz(2,3) + (1.0_kreal+s2)*vertXyz(2,4))

    c = 0.25_kreal * ((1.0_kreal-s1)*vertXyz(1,1) - (1.0_kreal-s1)*vertXyz(1,2) - &
         (1.0_kreal+s1)*vertXyz(1,3) + (1.0_kreal+s2)*vertXyz(1,4))

    d = 0.25_kreal * ((1.0_kreal-s1)*vertXyz(2,1) - (1.0_kreal-s1)*vertXyz(2,2) - &
         (1.0_kreal+s1)*vertXyz(2,3) + (1.0_kreal+s1)*vertXyz(2,4))
    bilinearPlaneJacobian = abs(a*d - b*c)
end function

!> @brief Lagrange interpolating polynomial, GLL basis function, cardinal function on 1d reference element [-1,1]
elemental function gll_cubic_basis1(x)
    real(kreal) :: gll_cubic_basis1
    real(kreal), intent(in) :: x
    gll_cubic_basis1 = (-1.0_kreal + x + 5.0_kreal * x**2 - 5.0_kreal * x**3) / 8.0_kreal
end function

!> @brief Lagrange interpolating polynomial, GLL basis function, cardinal function on 1d reference element [-1,1]
elemental function gll_cubic_basis2(x)
    real(kreal) :: gll_cubic_basis2
    real(kreal), intent(in) :: x
    gll_cubic_basis2 = 5.0_kreal * (x-1.0_kreal)*(x+1.0_kreal)* (r5 *x-1.0_kreal) / 8.0_kreal
end function

!> @brief Lagrange interpolating polynomial, GLL basis function, cardinal function on 1d reference element [-1,1]
elemental function gll_cubic_basis3(x)
    real(kreal) :: gll_cubic_basis3
    real(kreal), intent(in) :: x
    gll_cubic_basis3 = -r5 * (x-1.0_kreal)*(x+1.0_kreal)*(r5 + 5.0_kreal * x) / 8.0_kreal
end function

!> @brief Lagrange interpolating polynomial, GLL basis function, cardinal function on 1d reference element [-1,1]
elemental function gll_cubic_basis4(x)
    real(kreal) :: gll_cubic_basis4
    real(kreal), intent(in) :: x
    gll_cubic_basis4 = (-1.0_kreal - x + 5.0_kreal *x**2 + 5.0_kreal*x**3)/8.0_kreal
end function

end module


      SUBROUTINE SB10FD( N, M, NP, NCON, NMEAS, GAMMA, A, LDA, B, LDB,
     $                   C, LDC, D, LDD, AK, LDAK, BK, LDBK, CK, LDCK,
     $                   DK, LDDK, RCOND, TOL, IWORK, DWORK, LDWORK,
     $                   BWORK, INFO )
C
C     SLICOT RELEASE 5.7.
C
C     Copyright (c) 2002-2020 NICONET e.V.
C
C     PURPOSE
C
C     To compute the matrices of an H-infinity (sub)optimal n-state
C     controller
C
C              | AK | BK |
C          K = |----|----|,
C              | CK | DK |
C
C     using modified Glover's and Doyle's 1988 formulas, for the system
C
C                   | A  | B1  B2  |   | A | B |
C               P = |----|---------| = |---|---|
C                   | C1 | D11 D12 |   | C | D |
C                   | C2 | D21 D22 |
C
C     and for a given value of gamma, where B2 has as column size the
C     number of control inputs (NCON) and C2 has as row size the number
C     of measurements (NMEAS) being provided to the controller.
C
C     It is assumed that
C
C     (A1) (A,B2) is stabilizable and (C2,A) is detectable,
C
C     (A2) D12 is full column rank and D21 is full row rank,
C
C     (A3) | A-j*omega*I  B2  | has full column rank for all omega,
C          |    C1        D12 |
C
C     (A4) | A-j*omega*I  B1  |  has full row rank for all omega.
C          |    C2        D21 |
C
C     ARGUMENTS
C
C     Input/Output Parameters
C
C     N       (input) INTEGER
C             The order of the system.  N >= 0.
C
C     M       (input) INTEGER
C             The column size of the matrix B.  M >= 0.
C
C     NP      (input) INTEGER
C             The row size of the matrix C.  NP >= 0.
C
C     NCON    (input) INTEGER
C             The number of control inputs (M2).  M >= NCON >= 0,
C             NP-NMEAS >= NCON.
C
C     NMEAS   (input) INTEGER
C             The number of measurements (NP2).  NP >= NMEAS >= 0,
C             M-NCON >= NMEAS.
C
C     GAMMA   (input) DOUBLE PRECISION
C             The value of gamma. It is assumed that gamma is
C             sufficiently large so that the controller is admissible.
C             GAMMA >= 0.
C
C     A       (input) DOUBLE PRECISION array, dimension (LDA,N)
C             The leading N-by-N part of this array must contain the
C             system state matrix A.
C
C     LDA     INTEGER
C             The leading dimension of the array A.  LDA >= max(1,N).
C
C     B       (input) DOUBLE PRECISION array, dimension (LDB,M)
C             The leading N-by-M part of this array must contain the
C             system input matrix B.
C
C     LDB     INTEGER
C             The leading dimension of the array B.  LDB >= max(1,N).
C
C     C       (input) DOUBLE PRECISION array, dimension (LDC,N)
C             The leading NP-by-N part of this array must contain the
C             system output matrix C.
C
C     LDC     INTEGER
C             The leading dimension of the array C.  LDC >= max(1,NP).
C
C     D       (input) DOUBLE PRECISION array, dimension (LDD,M)
C             The leading NP-by-M part of this array must contain the
C             system input/output matrix D.
C
C     LDD     INTEGER
C             The leading dimension of the array D.  LDD >= max(1,NP).
C
C     AK      (output) DOUBLE PRECISION array, dimension (LDAK,N)
C             The leading N-by-N part of this array contains the
C             controller state matrix AK.
C
C     LDAK    INTEGER
C             The leading dimension of the array AK.  LDAK >= max(1,N).
C
C     BK      (output) DOUBLE PRECISION array, dimension (LDBK,NMEAS)
C             The leading N-by-NMEAS part of this array contains the
C             controller input matrix BK.
C
C     LDBK    INTEGER
C             The leading dimension of the array BK.  LDBK >= max(1,N).
C
C     CK      (output) DOUBLE PRECISION array, dimension (LDCK,N)
C             The leading NCON-by-N part of this array contains the
C             controller output matrix CK.
C
C     LDCK    INTEGER
C             The leading dimension of the array CK.
C             LDCK >= max(1,NCON).
C
C     DK      (output) DOUBLE PRECISION array, dimension (LDDK,NMEAS)
C             The leading NCON-by-NMEAS part of this array contains the
C             controller input/output matrix DK.
C
C     LDDK    INTEGER
C             The leading dimension of the array DK.
C             LDDK >= max(1,NCON).
C
C     RCOND   (output) DOUBLE PRECISION array, dimension (4)
C             RCOND(1) contains the reciprocal condition number of the
C                      control transformation matrix;
C             RCOND(2) contains the reciprocal condition number of the
C                      measurement transformation matrix;
C             RCOND(3) contains an estimate of the reciprocal condition
C                      number of the X-Riccati equation;
C             RCOND(4) contains an estimate of the reciprocal condition
C                      number of the Y-Riccati equation.
C
C     Tolerances
C
C     TOL     DOUBLE PRECISION
C             Tolerance used for controlling the accuracy of the applied
C             transformations for computing the normalized form in
C             SLICOT Library routine SB10PD. Transformation matrices
C             whose reciprocal condition numbers are less than TOL are
C             not allowed. If TOL <= 0, then a default value equal to
C             sqrt(EPS) is used, where EPS is the relative machine
C             precision.
C
C     Workspace
C
C     IWORK   INTEGER array, dimension (LIWORK), where
C             LIWORK = max(2*max(N,M-NCON,NP-NMEAS,NCON),N*N)
C
C     DWORK   DOUBLE PRECISION array, dimension (LDWORK)
C             On exit, if INFO = 0, DWORK(1) contains the optimal
C             LDWORK.
C
C     LDWORK  INTEGER
C             The dimension of the array DWORK.
C             LDWORK >= N*M + NP*(N+M) + M2*M2 + NP2*NP2 +
C                       max(1,LW1,LW2,LW3,LW4,LW5,LW6), where
C             LW1 = (N+NP1+1)*(N+M2) + max(3*(N+M2)+N+NP1,5*(N+M2)),
C             LW2 = (N+NP2)*(N+M1+1) + max(3*(N+NP2)+N+M1,5*(N+NP2)),
C             LW3 = M2 + NP1*NP1 + max(NP1*max(N,M1),3*M2+NP1,5*M2),
C             LW4 = NP2 + M1*M1 + max(max(N,NP1)*M1,3*NP2+M1,5*NP2),
C             LW5 = 2*N*N + N*(M+NP) +
C                   max(1,M*M + max(2*M1,3*N*N+max(N*M,10*N*N+12*N+5)),
C                       NP*NP + max(2*NP1,3*N*N +
C                                   max(N*NP,10*N*N+12*N+5))),
C             LW6 = 2*N*N + N*(M+NP) +
C                   max(1, M2*NP2 + NP2*NP2 + M2*M2 +
C                       max(D1*D1 + max(2*D1, (D1+D2)*NP2),
C                           D2*D2 + max(2*D2, D2*M2), 3*N,
C                           N*(2*NP2 + M2) +
C                           max(2*N*M2, M2*NP2 +
C                                       max(M2*M2+3*M2, NP2*(2*NP2+
C                                              M2+max(NP2,N)))))),
C             with D1 = NP1 - M2, D2 = M1 - NP2,
C                 NP1 = NP - NP2, M1 = M - M2.
C             For good performance, LDWORK must generally be larger.
C             Denoting Q = max(M1,M2,NP1,NP2), an upper bound is
C             2*Q*(3*Q+2*N)+max(1,(N+Q)*(N+Q+6),Q*(Q+max(N,Q,5)+1),
C               2*N*(N+2*Q)+max(1,4*Q*Q+
C                               max(2*Q,3*N*N+max(2*N*Q,10*N*N+12*N+5)),
C                                 Q*(3*N+3*Q+max(2*N,4*Q+max(N,Q))))).
C
C     BWORK   LOGICAL array, dimension (2*N)
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             = 1:  if the matrix | A-j*omega*I  B2  | had not full
C                                 |    C1        D12 |
C                   column rank in respect to the tolerance EPS;
C             = 2:  if the matrix | A-j*omega*I  B1  |  had not full row
C                                 |    C2        D21 |
C                   rank in respect to the tolerance EPS;
C             = 3:  if the matrix D12 had not full column rank in
C                   respect to the tolerance TOL;
C             = 4:  if the matrix D21 had not full row rank in respect
C                   to the tolerance TOL;
C             = 5:  if the singular value decomposition (SVD) algorithm
C                   did not converge (when computing the SVD of one of
C                   the matrices |A   B2 |, |A   B1 |, D12 or D21).
C                                |C1  D12|  |C2  D21|
C             = 6:  if the controller is not admissible (too small value
C                   of gamma);
C             = 7:  if the X-Riccati equation was not solved
C                   successfully (the controller is not admissible or
C                   there are numerical difficulties);
C             = 8:  if the Y-Riccati equation was not solved
C                   successfully (the controller is not admissible or
C                   there are numerical difficulties);
C             = 9:  if the determinant of Im2 + Tu*D11HAT*Ty*D22 is
C                   zero [3].
C
C     METHOD
C
C     The routine implements the Glover's and Doyle's 1988 formulas [1],
C     [2] modified to improve the efficiency as described in [3].
C
C     REFERENCES
C
C     [1] Glover, K. and Doyle, J.C.
C         State-space formulae for all stabilizing controllers that
C         satisfy an Hinf norm bound and relations to risk sensitivity.
C         Systems and Control Letters, vol. 11, pp. 167-172, 1988.
C
C     [2] Balas, G.J., Doyle, J.C., Glover, K., Packard, A., and
C         Smith, R.
C         mu-Analysis and Synthesis Toolbox.
C         The MathWorks Inc., Natick, Mass., 1995.
C
C     [3] Petkov, P.Hr., Gu, D.W., and Konstantinov, M.M.
C         Fortran 77 routines for Hinf and H2 design of continuous-time
C         linear control systems.
C         Rep. 98-14, Department of Engineering, Leicester University,
C         Leicester, U.K., 1998.
C
C     NUMERICAL ASPECTS
C
C     The accuracy of the result depends on the condition numbers of the
C     input and output transformations and on the condition numbers of
C     the two Riccati equations, as given by the values of RCOND(1),
C     RCOND(2), RCOND(3) and RCOND(4), respectively.
C
C     CONTRIBUTORS
C
C     P.Hr. Petkov, D.W. Gu and M.M. Konstantinov, October 1998.
C
C     REVISIONS
C
C     V. Sima, Research Institute for Informatics, Bucharest, May 1999,
C     Sept. 1999, Feb. 2000.
C
C     KEYWORDS
C
C     Algebraic Riccati equation, H-infinity optimal control, robust
C     control.
C
C     ******************************************************************
C
C     .. Parameters ..
      DOUBLE PRECISION   ZERO, ONE
      PARAMETER          ( ZERO = 0.0D+0, ONE = 1.0D+0 )
C     ..
C     .. Scalar Arguments ..
      INTEGER            INFO, LDA, LDAK, LDB, LDBK, LDC, LDCK, LDD,
     $                   LDDK, LDWORK, M, N, NCON, NMEAS, NP
      DOUBLE PRECISION   GAMMA, TOL
C     ..
C     .. Array Arguments ..
      LOGICAL            BWORK( * )
      INTEGER            IWORK( * )
      DOUBLE PRECISION   A( LDA, * ), AK( LDAK, * ), B( LDB, * ),
     $                   BK( LDBK, * ), C( LDC, * ), CK( LDCK, * ),
     $                   D( LDD, * ), DK( LDDK, * ), DWORK( * ),
     $                   RCOND( 4 )
C     ..
C     .. Local Scalars ..
      INTEGER            INFO2, IWC, IWD, IWF, IWH, IWRK, IWTU, IWTY,
     $                   IWX, IWY, LW1, LW2, LW3, LW4, LW5, LW6,
     $                   LWAMAX, M1, M2, MINWRK, ND1, ND2, NP1, NP2
      DOUBLE PRECISION   TOLL
C     ..
C     .. External Functions ..
      DOUBLE PRECISION   DLAMCH
      EXTERNAL           DLAMCH
C     ..
C     .. External Subroutines ..
      EXTERNAL           DLACPY, SB10PD, SB10QD, SB10RD, XERBLA
C     ..
C     .. Intrinsic Functions ..
      INTRINSIC          DBLE, INT, MAX, SQRT
C     ..
C     .. Executable Statements ..
C
C     Decode and Test input parameters.
C
      M1  = M - NCON
      M2  = NCON
      NP1 = NP - NMEAS
      NP2 = NMEAS
C
      INFO = 0
      IF( N.LT.0 ) THEN
         INFO = -1
      ELSE IF( M.LT.0 ) THEN
         INFO = -2
      ELSE IF( NP.LT.0 ) THEN
         INFO = -3
      ELSE IF( NCON.LT.0 .OR. M1.LT.0 .OR. M2.GT.NP1 ) THEN
         INFO = -4
      ELSE IF( NMEAS.LT.0 .OR. NP1.LT.0 .OR. NP2.GT.M1 ) THEN
         INFO = -5
      ELSE IF( GAMMA.LT.ZERO ) THEN
         INFO = -6
      ELSE IF( LDA.LT.MAX( 1, N ) ) THEN
         INFO = -8
      ELSE IF( LDB.LT.MAX( 1, N ) ) THEN
         INFO = -10
      ELSE IF( LDC.LT.MAX( 1, NP ) ) THEN
         INFO = -12
      ELSE IF( LDD.LT.MAX( 1, NP ) ) THEN
         INFO = -14
      ELSE IF( LDAK.LT.MAX( 1, N ) ) THEN
         INFO = -16
      ELSE IF( LDBK.LT.MAX( 1, N ) ) THEN
         INFO = -18
      ELSE IF( LDCK.LT.MAX( 1, M2 ) ) THEN
         INFO = -20
      ELSE IF( LDDK.LT.MAX( 1, M2 ) ) THEN
         INFO = -22
      ELSE
C
C        Compute workspace.
C
         ND1 = NP1 - M2
         ND2 = M1 - NP2
         LW1 = ( N + NP1 + 1 )*( N + M2 ) + MAX( 3*( N + M2 ) + N + NP1,
     $                                           5*( N + M2 ) )
         LW2 = ( N + NP2 )*( N + M1 + 1 ) + MAX( 3*( N + NP2 ) + N +
     $                                           M1, 5*( N + NP2 ) )
         LW3 = M2 + NP1*NP1 + MAX( NP1*MAX( N, M1 ), 3*M2 + NP1, 5*M2 )
         LW4 = NP2 + M1*M1  + MAX( MAX( N, NP1 )*M1, 3*NP2 + M1, 5*NP2 )
         LW5 = 2*N*N + N*( M + NP ) +
     $         MAX( 1, M*M + MAX( 2*M1, 3*N*N +
     $                            MAX( N*M, 10*N*N + 12*N + 5 ) ),
     $              NP*NP + MAX( 2*NP1, 3*N*N +
     $                           MAX( N*NP, 10*N*N + 12*N + 5 ) ) )
         LW6 = 2*N*N + N*( M + NP ) +
     $         MAX( 1, M2*NP2 + NP2*NP2 + M2*M2 +
     $                 MAX( ND1*ND1 + MAX( 2*ND1, ( ND1 + ND2 )*NP2 ),
     $                      ND2*ND2 + MAX( 2*ND2, ND2*M2 ), 3*N,
     $                      N*( 2*NP2 + M2 ) +
     $                      MAX( 2*N*M2, M2*NP2 +
     $                              MAX( M2*M2 + 3*M2, NP2*( 2*NP2 +
     $                                   M2 + MAX( NP2, N ) ) ) ) ) )
         MINWRK = N*M + NP*( N + M ) + M2*M2 + NP2*NP2 +
     $            MAX( 1, LW1, LW2, LW3, LW4, LW5, LW6 )
         IF( LDWORK.LT.MINWRK )
     $      INFO = -27
      END IF
      IF( INFO.NE.0 ) THEN
         CALL XERBLA( 'SB10FD', -INFO )
         RETURN
      END IF
C
C     Quick return if possible.
C
      IF( N.EQ.0 .OR. M.EQ.0 .OR. NP.EQ.0 .OR. M1.EQ.0 .OR. M2.EQ.0
     $    .OR. NP1.EQ.0 .OR. NP2.EQ.0 ) THEN
         RCOND( 1 ) = ONE
         RCOND( 2 ) = ONE
         RCOND( 3 ) = ONE
         RCOND( 4 ) = ONE
         DWORK( 1 ) = ONE
         RETURN
      END IF
C
      TOLL = TOL
      IF( TOLL.LE.ZERO ) THEN
C
C        Set the default value of the tolerance.
C
         TOLL = SQRT( DLAMCH( 'Epsilon' )  )
      END IF
C
C     Workspace usage.
C
      IWC  = 1 + N*M
      IWD  = IWC + NP*N
      IWTU = IWD + NP*M
      IWTY = IWTU + M2*M2
      IWRK = IWTY + NP2*NP2
C
      CALL DLACPY( 'Full', N, M, B, LDB, DWORK, N )
      CALL DLACPY( 'Full', NP, N, C, LDC, DWORK( IWC ), NP )
      CALL DLACPY( 'Full', NP, M, D, LDD, DWORK( IWD ), NP )
C
C     Transform the system so that D12 and D21 satisfy the formulas
C     in the computation of the Hinf (sub)optimal controller.
C
      CALL SB10PD( N, M, NP, NCON, NMEAS, A, LDA, DWORK, N,
     $             DWORK( IWC ), NP, DWORK( IWD ), NP, DWORK( IWTU ),
     $             M2, DWORK( IWTY ), NP2, RCOND, TOLL, DWORK( IWRK ),
     $             LDWORK-IWRK+1, INFO2 )
      IF( INFO2.GT.0 ) THEN
         INFO = INFO2
         RETURN
      END IF
      LWAMAX = INT( DWORK( IWRK ) ) + IWRK - 1
C
      IWX  = IWRK
      IWY  = IWX + N*N
      IWF  = IWY + N*N
      IWH  = IWF + M*N
      IWRK = IWH + N*NP
C
C     Compute the (sub)optimal state feedback and output injection
C     matrices.
C
      CALL SB10QD( N, M, NP, NCON, NMEAS, GAMMA, A, LDA, DWORK, N,
     $             DWORK( IWC ), NP, DWORK( IWD ), NP, DWORK( IWF ),
     $             M, DWORK( IWH ), N, DWORK( IWX ), N, DWORK( IWY ),
     $             N, RCOND(3), IWORK, DWORK( IWRK ), LDWORK-IWRK+1,
     $             BWORK, INFO2 )
      IF( INFO2.GT.0 ) THEN
         INFO = INFO2 + 5
         RETURN
      END IF
      LWAMAX = MAX( INT( DWORK( IWRK ) ) + IWRK - 1, LWAMAX )
C
C     Compute the Hinf (sub)optimal controller.
C
      CALL SB10RD( N, M, NP, NCON, NMEAS, GAMMA, A, LDA, DWORK, N,
     $             DWORK( IWC ), NP, DWORK( IWD ), NP, DWORK( IWF ),
     $             M, DWORK( IWH ), N, DWORK( IWTU ), M2, DWORK( IWTY ),
     $             NP2, DWORK( IWX ), N, DWORK( IWY ), N, AK, LDAK, BK,
     $             LDBK, CK, LDCK, DK, LDDK, IWORK, DWORK( IWRK ),
     $             LDWORK-IWRK+1, INFO2 )
      IF( INFO2.EQ.1 ) THEN
         INFO = 6
         RETURN
      ELSE IF( INFO2.EQ.2 ) THEN
         INFO = 9
         RETURN
      END IF
      LWAMAX = MAX( INT( DWORK( IWRK ) ) + IWRK - 1, LWAMAX )
C
      DWORK( 1 ) = DBLE( LWAMAX )
      RETURN
C *** Last line of SB10FD ***
      END


module cov_compute

contains

subroutine calc_cov_elem_spin0(ac_bd, ad_bc, l1, l2, nlmax, elems)
    implicit none
    integer, intent(in)   :: l1, l2, nlmax
    real(8), intent(in)   ::  ac_bd(:),ad_bc(:)
    real(8), intent(inout):: elems(2)
    real(8) :: thrcof0(2*(nlmax+1)), l1f, l2f
    integer :: info, l3, wlmin, wlmax, i
    call drc3jj(dble(l1), dble(l2), 0d0, 0d0, l1f, l2f, thrcof0, size(thrcof0), info)
    wlmin = int(l1f)
    wlmax = min(nlmax+1, int(l2f))
    elems = 0
    do l3 = wlmin, wlmax
        i = l3 - wlmin + 1
        elems(1) = elems(1) + ac_bd(l3+1)*thrcof0(i)**2
        elems(2) = elems(2) + ad_bc(l3+1)*thrcof0(i)**2
    end do
end subroutine

subroutine calc_cov_elem_spin0and2_simple(TaTcTbTd, TaTdTbTc, PaPcPbPd, PaPdPbPc, &
                                        & TaTcPbPd, TaPdPbTc, PaPcTbTd, PaTdTbPc, &
                                        & TaPcTbPd, TaPdTbPc, TaTcTbPd, TaPdTbTc, &
                                        & TaPcTbTd, TaTdTbPc, TaPcPbTd, TaTdPbPc, &
                                        & TaPcPbPd, TaPdPbPc, PaPcTbPd, PaPdTbPc, &
                                        & TaTcPbTd, TaTdPbTc, PaTcTbTd, PaTdTbTc, &
                                        & PaTcPbTd, PaTdPbTc, PaTcTbPd, PaPdTbTc, &
                                        & PaTcPbPd, PaPdPbTc, PaPcPbTd, PaTdPbPc, &
                                        doPlanck, l1, l2, nlmax, elems)
    implicit none
    integer, intent(in)    :: doPlanck, l1, l2, nlmax
    real(8), intent(in)    :: TaTcTbTd(:), TaTdTbTc(:), PaPcPbPd(:), PaPdPbPc(:)
    real(8), intent(in)    :: TaTcPbPd(:), TaPdPbTc(:), PaPcTbTd(:), PaTdTbPc(:)
    real(8), intent(in)    :: TaPcTbPd(:), TaPdTbPc(:), TaTcTbPd(:), TaPdTbTc(:)
    real(8), intent(in)    :: TaPcTbTd(:), TaTdTbPc(:), TaPcPbTd(:), TaTdPbPc(:)
    real(8), intent(in)    :: TaPcPbPd(:), TaPdPbPc(:), PaPcTbPd(:), PaPdTbPc(:)
    real(8), intent(in)    :: TaTcPbTd(:), TaTdPbTc(:), PaTcTbTd(:), PaTdTbTc(:)
    real(8), intent(in)    :: PaTcPbTd(:), PaTdPbTc(:), PaTcTbPd(:), PaPdTbTc(:)
    real(8), intent(in)    :: PaTcPbPd(:), PaPdPbTc(:), PaPcPbTd(:), PaTdPbPc(:)
    real(8), intent(inout):: elems(32)
    real(8) :: thrcof0(2*(nlmax+1)),thrcof1(2*(nlmax+1)), l1f, l2f
    integer :: info, l3, wlmin, wlmax, i

    if (doPlanck .eq. 0) then
        call drc3jj(dble(l1), dble(l2), 0d0, 0d0, l1f, l2f, thrcof0, size(thrcof0), info)
        wlmin = int(l1f)
        wlmax = min(nlmax+1, int(l2f))
        elems = 0
        do l3 = wlmin, wlmax
            i = l3 - wlmin + 1
            elems(1) = elems(1) + (TaTcTbTd(l3+1)*thrcof0(i)**2d0)
            elems(2) = elems(2) + (TaTdTbTc(l3+1)*thrcof0(i)**2d0)
            elems(3) = elems(3) + (PaPcPbPd(l3+1)*thrcof0(i)**2d0)
            elems(4) = elems(4) + (PaPdPbPc(l3+1)*thrcof0(i)**2d0)
            elems(5) = elems(5) + (TaTcPbPd(l3+1)*thrcof0(i)**2d0)
            elems(6) = elems(6) + (TaPdPbTc(l3+1)*thrcof0(i)**2d0)
            elems(7) = elems(7) + (PaPcTbTd(l3+1)*thrcof0(i)**2d0)
            elems(8) = elems(8) + (PaTdTbPc(l3+1)*thrcof0(i)**2d0)
            elems(9) = elems(9) + (TaPcTbPd(l3+1)*thrcof0(i)**2d0)
            elems(10) = elems(10) + (TaPdTbPc(l3+1)*thrcof0(i)**2d0)
            elems(11) = elems(11) + (TaTcTbPd(l3+1)*thrcof0(i)**2d0)
            elems(12) = elems(12) + (TaPdTbTc(l3+1)*thrcof0(i)**2d0)
            elems(13) = elems(13) + (TaPcTbTd(l3+1)*thrcof0(i)**2d0)
            elems(14) = elems(14) + (TaTdTbPc(l3+1)*thrcof0(i)**2d0)
            elems(15) = elems(15) + (TaPcPbTd(l3+1)*thrcof0(i)**2d0)
            elems(16) = elems(16) + (TaTdPbPc(l3+1)*thrcof0(i)**2d0)
            elems(17) = elems(17) + (TaPcPbPd(l3+1)*thrcof0(i)**2d0)
            elems(18) = elems(18) + (TaPdPbPc(l3+1)*thrcof0(i)**2d0)
            elems(19) = elems(19) + (PaPcTbPd(l3+1)*thrcof0(i)**2d0)
            elems(20) = elems(20) + (PaPdTbPc(l3+1)*thrcof0(i)**2d0)
            elems(21) = elems(21) + (TaTcPbTd(l3+1)*thrcof0(i)**2d0)
            elems(22) = elems(22) + (TaTdPbTc(l3+1)*thrcof0(i)**2d0)
            elems(23) = elems(23) + (PaTcTbTd(l3+1)*thrcof0(i)**2d0)
            elems(24) = elems(24) + (PaTdTbTc(l3+1)*thrcof0(i)**2d0)
            elems(25) = elems(25) + (PaTcPbTd(l3+1)*thrcof0(i)**2d0)
            elems(26) = elems(26) + (PaTdPbTc(l3+1)*thrcof0(i)**2d0)
            elems(27) = elems(27) + (PaTcTbPd(l3+1)*thrcof0(i)**2d0)
            elems(28) = elems(28) + (PaPdTbTc(l3+1)*thrcof0(i)**2d0)
            elems(29) = elems(29) + (PaTcPbPd(l3+1)*thrcof0(i)**2d0)
            elems(30) = elems(30) + (PaPdPbTc(l3+1)*thrcof0(i)**2d0)
            elems(31) = elems(31) + (PaPcPbTd(l3+1)*thrcof0(i)**2d0)
            elems(32) = elems(32) + (PaTdPbPc(l3+1)*thrcof0(i)**2d0)
        end do
    else
        call drc3jj(dble(l1), dble(l2), 0d0, 0d0, l1f, l2f, thrcof0, size(thrcof0), info)
        call drc3jj(dble(l1), dble(l2), -2d0, 2d0, l1f, l2f, thrcof1, size(thrcof1), info)

        wlmin = int(l1f)
        wlmax = min(nlmax+1, int(l2f))
        elems = 0
        do l3 = wlmin, wlmax
            i = l3 - wlmin + 1
            elems(1) = elems(1) + (TaTcTbTd(l3+1)*thrcof0(i)**2d0)
            elems(2) = elems(2) + (TaTdTbTc(l3+1)*thrcof0(i)**2d0)
            elems(3) = elems(3) + (PaPcPbPd(l3+1)*thrcof1(i)**2*(1+(-1)**(l1+l2+l3))/2)
            elems(4) = elems(4) + (PaPdPbPc(l3+1)*thrcof1(i)**2*(1+(-1)**(l1+l2+l3))/2)
            elems(5) = elems(5) + (TaTcPbPd(l3+1)*thrcof0(i)*thrcof1(i))
            elems(6) = elems(6) + (TaPdPbTc(l3+1)*thrcof0(i)**2d0)
            elems(7) = elems(7) + (PaPcTbTd(l3+1)*thrcof0(i)*thrcof1(i))
            elems(8) = elems(8) + (PaTdTbPc(l3+1)*thrcof0(i)**2d0)
            elems(9) = elems(9) + (TaPcTbPd(l3+1)*thrcof0(i)**2d0)
            elems(10) = elems(10) + (TaPdTbPc(l3+1)*thrcof0(i)**2d0)
            elems(11) = elems(11) + (TaTcTbPd(l3+1)*thrcof0(i)**2d0)
            elems(12) = elems(12) + (TaPdTbTc(l3+1)*thrcof0(i)**2d0)
            elems(13) = elems(13) + (TaPcTbTd(l3+1)*thrcof0(i)**2d0)
            elems(14) = elems(14) + (TaTdTbPc(l3+1)*thrcof0(i)**2d0)
            elems(15) = elems(15) + (TaPcPbTd(l3+1)*thrcof0(i)**2d0)
            elems(16) = elems(16) + (TaTdPbPc(l3+1)*thrcof0(i)*thrcof1(i))
            elems(17) = elems(17) + (TaPcPbPd(l3+1)*thrcof1(i)**2*(1+(-1)**(l1+l2+l3))/2)
            elems(18) = elems(18) + (TaPdPbPc(l3+1)*thrcof1(i)**2*(1+(-1)**(l1+l2+l3))/2)
            elems(19) = elems(19) + (PaPcTbPd(l3+1)*thrcof1(i)**2*(1+(-1)**(l1+l2+l3))/2)
            elems(20) = elems(20) + (PaPdTbPc(l3+1)*thrcof1(i)**2*(1+(-1)**(l1+l2+l3))/2)
            elems(21) = elems(21) + (TaTcPbTd(l3+1)*thrcof0(i)**2d0)
            elems(22) = elems(22) + (TaTdPbTc(l3+1)*thrcof0(i)**2d0)
            elems(23) = elems(23) + (PaTcTbTd(l3+1)*thrcof0(i)**2d0)
            elems(24) = elems(24) + (PaTdTbTc(l3+1)*thrcof0(i)**2d0)
            elems(25) = elems(25) + (PaTcPbTd(l3+1)*thrcof0(i)**2d0)
            elems(26) = elems(26) + (PaTdPbTc(l3+1)*thrcof0(i)**2d0)
            elems(27) = elems(27) + (PaTcTbPd(l3+1)*thrcof0(i)**2d0)
            elems(28) = elems(28) + (PaPdTbTc(l3+1)*thrcof0(i)*thrcof1(i))
            elems(29) = elems(29) + (PaTcPbPd(l3+1)*thrcof1(i)**2*(1+(-1)**(l1+l2+l3))/2)
            elems(30) = elems(30) + (PaPdPbTc(l3+1)*thrcof1(i)**2*(1+(-1)**(l1+l2+l3))/2)
            elems(31) = elems(31) + (PaPcPbTd(l3+1)*thrcof1(i)**2*(1+(-1)**(l1+l2+l3))/2)
            elems(32) = elems(32) + (PaTdPbPc(l3+1)*thrcof1(i)**2*(1+(-1)**(l1+l2+l3))/2)

        end do
    end if

end subroutine

subroutine calc_cov_spin0(ac_bd, ad_bc, l_exact, l_band, l_toeplitz, cov_array)
    implicit none
    real(8), intent(in)    :: ac_bd(:),ad_bc(:)
    integer, intent(in)   :: l_exact, l_band, l_toeplitz
    real(8), intent(inout) :: cov_array(:,:,:)
    real(8), parameter     :: pi = 3.14159265358979323846264d0
    integer :: l1, l2, nlmax, lmax_band
    nlmax = size(cov_array,1)-1

    !$omp parallel do private(l2, l1) schedule(dynamic)
    do l1 = 2, min(nlmax, l_exact)
        do l2 = l1, nlmax
            call calc_cov_elem_spin0(ac_bd, ad_bc, l1, l2, nlmax, cov_array(l1-1, l2-1,:))
        end do
    end do

    if (l_exact .lt. nlmax) then

        !$omp parallel do private(l2, l1) schedule(dynamic)
        do l1 = l_exact+1, l_toeplitz

            if (l1 .lt. l_toeplitz) then
                lmax_band = min(l1 + l_band, nlmax)
            else
                lmax_band = nlmax
            end if

            do l2 = l1, lmax_band
                call calc_cov_elem_spin0(ac_bd, ad_bc, l1, l2, nlmax, cov_array(l1-1, l2-1,:))
            end do
        end do

    	if (l_toeplitz .lt. nlmax) then
            !$omp parallel do private(l1) schedule(dynamic)
            do l1 = l_toeplitz+1, nlmax
                call calc_cov_elem_spin0(ac_bd, ad_bc, l1, l1, nlmax, cov_array(l1-1, l1-1,:))
            end do
        end if
    end if

end subroutine

subroutine calc_cov_spin0and2_simple(TaTcTbTd, TaTdTbTc, PaPcPbPd, PaPdPbPc, &
                                   & TaTcPbPd, TaPdPbTc, PaPcTbTd, PaTdTbPc, &
                                   & TaPcTbPd, TaPdTbPc, TaTcTbPd, TaPdTbTc, &
                                   & TaPcTbTd, TaTdTbPc, TaPcPbTd, TaTdPbPc, &
                                   & TaPcPbPd, TaPdPbPc, PaPcTbPd, PaPdTbPc, &
                                   & TaTcPbTd, TaTdPbTc, PaTcTbTd, PaTdTbTc, &
                                   & PaTcPbTd, PaTdPbTc, PaTcTbPd, PaPdTbTc, &
                                   & PaTcPbPd, PaPdPbTc, PaPcPbTd, PaTdPbPc, &
                                   & doPlanck, l_exact, l_band, l_toeplitz, cov_array)

    implicit none
    real(8), intent(in)    :: TaTcTbTd(:), TaTdTbTc(:), PaPcPbPd(:), PaPdPbPc(:)
    real(8), intent(in)    :: TaTcPbPd(:), TaPdPbTc(:), PaPcTbTd(:), PaTdTbPc(:)
    real(8), intent(in)    :: TaPcTbPd(:), TaPdTbPc(:), TaTcTbPd(:), TaPdTbTc(:)
    real(8), intent(in)    :: TaPcTbTd(:), TaTdTbPc(:), TaPcPbTd(:), TaTdPbPc(:)
    real(8), intent(in)    :: TaPcPbPd(:), TaPdPbPc(:), PaPcTbPd(:), PaPdTbPc(:)
    real(8), intent(in)    :: TaTcPbTd(:), TaTdPbTc(:), PaTcTbTd(:), PaTdTbTc(:)
    real(8), intent(in)    :: PaTcPbTd(:), PaTdPbTc(:), PaTcTbPd(:), PaPdTbTc(:)
    real(8), intent(in)    :: PaTcPbPd(:), PaPdPbTc(:), PaPcPbTd(:), PaTdPbPc(:)
    integer, intent(in)    :: l_exact, l_band, l_toeplitz, doPlanck
    real(8), intent(inout) :: cov_array(:,:,:)
    real(8), parameter     :: pi = 3.14159265358979323846264d0
    integer :: l1, l2, nlmax, lmax_band
    nlmax = size(cov_array,1)-1

    !$omp parallel do private(l2, l1) schedule(dynamic)
    do l1 = 2, min(nlmax, l_exact)
        do l2 = l1, nlmax
            call calc_cov_elem_spin0and2_simple(TaTcTbTd, TaTdTbTc, PaPcPbPd, PaPdPbPc, &
                                              & TaTcPbPd, TaPdPbTc, PaPcTbTd, PaTdTbPc, &
                                              & TaPcTbPd, TaPdTbPc, TaTcTbPd, TaPdTbTc, &
                                              & TaPcTbTd, TaTdTbPc, TaPcPbTd, TaTdPbPc, &
                                              & TaPcPbPd, TaPdPbPc, PaPcTbPd, PaPdTbPc, &
                                              & TaTcPbTd, TaTdPbTc, PaTcTbTd, PaTdTbTc, &
                                              & PaTcPbTd, PaTdPbTc, PaTcTbPd, PaPdTbTc, &
                                              & PaTcPbPd, PaPdPbTc, PaPcPbTd, PaTdPbPc, &
                                              doPlanck, l1, l2, nlmax, cov_array(l1-1, l2-1, :))
        end do
    end do

    if (l_exact .lt. nlmax) then
        !$omp parallel do private(l2, l1, lmax_band) schedule(dynamic)
        do l1 = l_exact + 1, l_toeplitz
            if (l1 .lt. l_toeplitz) then
                lmax_band = min(l1 + l_band, nlmax)
            else
                lmax_band = nlmax
            end if
            do l2 = l1, lmax_band
                call calc_cov_elem_spin0and2_simple(TaTcTbTd, TaTdTbTc, PaPcPbPd, PaPdPbPc, &
                                                  & TaTcPbPd, TaPdPbTc, PaPcTbTd, PaTdTbPc, &
                                                  & TaPcTbPd, TaPdTbPc, TaTcTbPd, TaPdTbTc, &
                                                  & TaPcTbTd, TaTdTbPc, TaPcPbTd, TaTdPbPc, &
                                                  & TaPcPbPd, TaPdPbPc, PaPcTbPd, PaPdTbPc, &
                                                  & TaTcPbTd, TaTdPbTc, PaTcTbTd, PaTdTbTc, &
                                                  & PaTcPbTd, PaTdPbTc, PaTcTbPd, PaPdTbTc, &
                                                  & PaTcPbPd, PaPdPbTc, PaPcPbTd, PaTdPbPc, &
                                                  doPlanck, l1, l2, nlmax, cov_array(l1-1, l2-1, :))

            end do
        end do

        if (l_toeplitz .lt. nlmax) then
            !$omp parallel do private(l1) schedule(dynamic)
            do l1 = l_toeplitz + 1, nlmax
                call calc_cov_elem_spin0and2_simple(TaTcTbTd, TaTdTbTc, PaPcPbPd, PaPdPbPc, &
                                                  & TaTcPbPd, TaPdPbTc, PaPcTbTd, PaTdTbPc, &
                                                  & TaPcTbPd, TaPdTbPc, TaTcTbPd, TaPdTbTc, &
                                                  & TaPcTbTd, TaTdTbPc, TaPcPbTd, TaTdPbPc, &
                                                  & TaPcPbPd, TaPdPbPc, PaPcTbPd, PaPdTbPc, &
                                                  & TaTcPbTd, TaTdPbTc, PaTcTbTd, PaTdTbTc, &
                                                  & PaTcPbTd, PaTdPbTc, PaTcTbPd, PaPdTbTc, &
                                                  & PaTcPbPd, PaPdPbTc, PaPcPbTd, PaTdPbPc, &
                                                  doPlanck, l1, l1, nlmax, cov_array(l1-1, l1-1, :))

            end do
        end if
    end if

end subroutine

end module


! A procedure argument can be intent(in out), which means
! that it is an input that can be overwritten. Functions
! should not have such arguments, but subroutines often do.
! Intent(out) means the argument is set to uninitialized upon
! entering the procedure.
!
program intent_in_out
implicit none
real :: x(3)
x = [1.,4.,5.]
call normalize(x)
print*,x ! 0.1 0.4 0.5
contains
subroutine normalize(x)
! scale x so that sum(x) = 1.
real, intent(in out) :: x(:)
real                 :: xsum
xsum = sum(x)
if (xsum /= 0) x = x/xsum
end subroutine normalize
end program intent_in_out


program ex

  use gabriel
  use MPI
  implicit none

  integer,parameter :: n=10
  integer,parameter :: s=2

  logical :: correct=.true.

  real,dimension(:),allocatable :: a,b
  integer ierr,rank,right,left,mpisize,i,j,k
  integer sz,r,oi,oo,e           ! offset in, offset out
  character :: szchar,aszchar
  type(distribution) :: d
  type(box) :: b1, b2

  call MPI_Init(ierr)
  call MPI_Comm_rank(MPI_COMM_WORLD,rank,ierr)
  call MPI_Comm_size(MPI_COMM_WORLD,mpisize,ierr)
  if (mpisize.gt.9)call MPI_Abort(MPI_COMM_WORLD,1,ierr)

  right=rank+1
  left=rank-1
  if (right.ge.mpisize)right=0
  if (left.lt.0)left=mpisize-1
  print*,'left,rank,right=',left,rank,right

  sz=mpisize*(mpisize+1)/2
  szchar=char(ichar('0')+sz)

  print*,sz,'=',szchar
  r=rank+1
  oi=0
  oo=0
  do i=1,rank
    oi=oi+i
    oo=oo+mpisize-i+1
  enddo
  allocate(a(1:r))
  allocate(b(1:mpisize-r+1))
  call b1%init(a,lbound(a),ubound(a),MPI_COMM_WORLD,offset=(/oi/),err=e)
  call b2%init(b,lbound(b),ubound(b),MPI_COMM_WORLD,offset=(/oo/),err=e)
  do i=1,r
    a(i)=i+oi
  enddo
  print*,'shape(a),shape(b)=',shape(a),shape(b)
  print*,'lb(a),lb(b)=',lbound(a),lbound(b)
  print*,'ub(a),ub(b)=',ubound(a),ubound(b)
  b=0.0

  write(*,'(a,4i3)')'Size,Rank=',mpisize,rank

  aszchar=char(ichar('0')+size(a,1))
  write(*,'(a,i3,a,'//aszchar//'f8.3)')'BEFORE Rank',rank,' data=',a(:)

  print*,'Create compositions..'
  print*,'Create transform..'
  call d%transform(b1,b2,err=e)

  print*,'Transform decomposition..'
  call d%update(a,b,err=e)

!  write(*,'(a,i3,a,i4,4f13.3)')' AFTER Rank',rank,' data=',b(1,:)
  aszchar=char(ichar('0')+size(b,1))
  write(*,'(a,i3,a,'//aszchar//'f5.1)')' AFTER Rank',rank,' data=',b(:)

!  do i=lbound(b,1),ubound(b,1)
!  do j=lbound(b,2),ubound(b,2)
!    if (b(i,j).ne.i*0.1+j*0.01) correct=.false.
!  enddo
!  enddo

  deallocate(a,b)

  call MPI_Finalize(ierr)
  if (.not.correct) stop 1

end


program vin
  integer :: val3(3)
  val3 = (/ 1, 2, 3 /) + (/4,5,6 /)
  print *,val3
end program vin


\ Numeric conversion and text I/O
\ The buffer for numeric conversion is just below the tib.
\ The output string grows downward in memory.

there
decimal
variable hld                            \ 2.3000 -- c-addr
32 equ bl                               \ 2.3010 -- char

frp0 equ numbuf                         \ buffer for numeric conversion

: decimal 10 base ! ;
: hex     16 base ! ;
:noname   count emit ;  ( xt )          \ send string to output device
: type    literal times drop ;          \ 2.3140 c-addr u --
: s>d     dup 0< ;                      \ 2.3150 n -- d
: space   bl emit ;                     \ 2.3160 --
: spaces  ['] space times ;             \ 2.3170 n --

\ Numeric conversion. `d.r` uses frame stack protection to prevent overflow
\ when the stacks have significant content. Since `d.r` ia typically at the
\ end of a definition, its tail call doesn't increase the stack.

: digit   dup -10 + 0< -7 and           \ 2.3180 n -- char
          + [char] 7 + ;
: <#      numbuf  hld ! ;               \ 2.3190 ud1 -- ud1
: hold    hld dup >r @ 1- dup r> ! c! ; \ 2.3200 char --
: _#_     um/mod swap digit hold ;      \ ud base -- u/base
: #       dup  base @ >r  if            \ 2.3210 ud1 -- ud2
              0 r@ um/mod r> swap
              >r _#_ r> exit
          then  r> _#_ 0
;
: #s      begin # 2dup or 0= until ;    \ 2.3220 ud1 -- ud2
: sign    0< if [char] - hold then ;    \ 2.3230 n --
: #>      2drop hld @ numbuf  over - ;  \ 2.3240 ud -- c-addr u
: s.r     over - spaces type ;          \ length width --
: d.r     3 stack(  >r dup >r dabs      \ 2.3250 d width --
          <# #s r> sign #> r> s.r )stack ;
: u.r     0 swap d.r ;                  \ 2.3260 u width --
: .r      >r s>d r> d.r ;               \ 2.3270 n width --
: d.      0 d.r space ;                 \ 2.3280 d --
: u.      0 d. ;                        \ 2.3290 u --
: ?       @ [ ;                         \ 2.3310 a --
: .       s>d d. ;                      \ 2.3300 n --
: <#>     >r  <# begin # next #s #> ;   \ ud digits-1
: h.2     1 [ ;
: h.x     base @ >r hex  0 swap <#> r>  \ 2.3320 u n --
          base !  type space ;

there swap - . .( instructions used by numeric output) cr


      subroutine daxpy(n, alpha, x, y)
      implicit none
      integer n
      double precision alpha
      double precision x(n), y(n)
Cf2py integer intent(in) :: n
Cf2py double precision intent(in) :: alpha
Cf2py double precision intent(in), depend(n) :: x(n)
Cf2py double precision intent(in,out), depend(n) :: y(n)
      integer i
      do i = 1, n
         y(i) = alpha * x(i) + y(i)
      enddo
      end subroutine

      subroutine daxpy_inplace(n, alpha, x, y)
      implicit none
      integer n
      double precision alpha
      double precision x(n), y(n)
Cf2py integer intent(in) :: n
Cf2py double precision intent(in) :: alpha
Cf2py double precision intent(in), depend(n) :: x(n)
Cf2py double precision intent(inout), depend(n) :: y(n)
      integer i
      do i = 1, n
         y(i) = alpha * x(i) + y(i)
      enddo
      end subroutine


! Created by EverLookNeverSee@GitHub on 4/14/20.
! This program gets two integer number from user and
! determines which one is maximum and which one is minimum.

program max_min
    implicit none
    ! declaring two integer variables
    integer :: number_1, number_2
    print *, "Enter two integer number: "
    ! getting user input
    read *, number_1, number_2
    if (number_1 > number_2) then
        print *, "maximum:", number_1, "minimum:", number_2
    elseif (number_2 > number_1) then
        print *, "maximum:", number_2, "minimum:", number_1
    else
        print *, number_1, "and", number_2, "is equal."
    end if
end program max_min


      SUBROUTINE MB04ND( UPLO, N, M, P, R, LDR, A, LDA, B, LDB, C, LDC,
     $                   TAU, DWORK )
C
C     SLICOT RELEASE 5.7.
C
C     Copyright (c) 2002-2020 NICONET e.V.
C
C     PURPOSE
C
C     To calculate an RQ factorization of the first block row and
C     apply the orthogonal transformations (from the right) also to the
C     second block row of a structured matrix, as follows
C                              _
C       [ A   R ]        [ 0   R ]
C       [       ] * Q' = [ _   _ ]
C       [ C   B ]        [ C   B ]
C                 _
C     where R and R are upper triangular. The matrix A can be full or
C     upper trapezoidal/triangular. The problem structure is exploited.
C
C     ARGUMENTS
C
C     Mode Parameters
C
C     UPLO    CHARACTER*1
C             Indicates if the matrix A is or not triangular as follows:
C             = 'U':  Matrix A is upper trapezoidal/triangular;
C             = 'F':  Matrix A is full.
C
C     Input/Output Parameters
C
C     N       (input) INTEGER                 _
C             The order of the matrices R and R.  N >= 0.
C
C     M       (input) INTEGER
C             The number of rows of the matrices B and C.  M >= 0.
C
C     P       (input) INTEGER
C             The number of columns of the matrices A and C.  P >= 0.
C
C     R       (input/output) DOUBLE PRECISION array, dimension (LDR,N)
C             On entry, the leading N-by-N upper triangular part of this
C             array must contain the upper triangular matrix R.
C             On exit, the leading N-by-N upper triangular part of this
C                                                        _
C             array contains the upper triangular matrix R.
C             The strict lower triangular part of this array is not
C             referenced.
C
C     LDR     INTEGER
C             The leading dimension of array R.  LDR >= MAX(1,N).
C
C     A       (input/output) DOUBLE PRECISION array, dimension (LDA,P)
C             On entry, if UPLO = 'F', the leading N-by-P part of this
C             array must contain the matrix A. For UPLO = 'U', if
C             N <= P, the upper triangle of the subarray A(1:N,P-N+1:P)
C             must contain the N-by-N upper triangular matrix A, and if
C             N >= P, the elements on and above the (N-P)-th subdiagonal
C             must contain the N-by-P upper trapezoidal matrix A.
C             On exit, if UPLO = 'F', the leading N-by-P part of this
C             array contains the trailing components (the vectors v, see
C             METHOD) of the elementary reflectors used in the
C             factorization. If UPLO = 'U', the upper triangle of the
C             subarray A(1:N,P-N+1:P) (if N <= P), or the elements on
C             and above the (N-P)-th subdiagonal (if N >= P), contain
C             the trailing components (the vectors v, see METHOD) of the
C             elementary reflectors used in the factorization.
C             The remaining elements are not referenced.
C
C     LDA     INTEGER
C             The leading dimension of array A.  LDA >= MAX(1,N).
C
C     B       (input/output) DOUBLE PRECISION array, dimension (LDB,N)
C             On entry, the leading M-by-N part of this array must
C             contain the matrix B.
C             On exit, the leading M-by-N part of this array contains
C                                 _
C             the computed matrix B.
C
C     LDB     INTEGER
C             The leading dimension of array B.  LDB >= MAX(1,M).
C
C     C       (input/output) DOUBLE PRECISION array, dimension (LDC,P)
C             On entry, the leading M-by-P part of this array must
C             contain the matrix C.
C             On exit, the leading M-by-P part of this array contains
C                                 _
C             the computed matrix C.
C
C     LDC     INTEGER
C             The leading dimension of array C.  LDC >= MAX(1,M).
C
C     TAU     (output) DOUBLE PRECISION array, dimension (N)
C             The scalar factors of the elementary reflectors used.
C
C     Workspace
C
C     DWORK   DOUBLE PRECISION array, dimension (MAX(N-1,M))
C
C     METHOD
C
C     The routine uses N Householder transformations exploiting the zero
C     pattern of the block matrix.  A Householder matrix has the form
C
C                                     ( 1 )
C        H  = I - tau *u *u',    u  = ( v ),
C         i          i  i  i      i   (  i)
C
C     where v  is a P-vector, if UPLO = 'F', or a min(N-i+1,P)-vector,
C            i
C     if UPLO = 'U'.  The components of v  are stored in the i-th row
C                                        i
C     of A, and tau  is stored in TAU(i), i = N,N-1,...,1.
C                  i
C     In-line code for applying Householder transformations is used
C     whenever possible (see MB04NY routine).
C
C     NUMERICAL ASPECTS
C
C     The algorithm is backward stable.
C
C     CONTRIBUTORS
C
C     V. Sima, Katholieke Univ. Leuven, Belgium, Apr. 1998.
C
C     REVISIONS
C
C     -
C
C     KEYWORDS
C
C     Elementary reflector, RQ factorization, orthogonal transformation.
C
C     ******************************************************************
C
C     .. Scalar Arguments ..
      CHARACTER         UPLO
      INTEGER           LDA, LDB, LDC, LDR, M, N, P
C     .. Array Arguments ..
      DOUBLE PRECISION  A(LDA,*), B(LDB,*), C(LDC,*), DWORK(*),
     $                  R(LDR,*), TAU(*)
C     .. Local Scalars ..
      LOGICAL           LUPLO
      INTEGER           I, IM, IP
C     .. External Functions ..
      LOGICAL           LSAME
      EXTERNAL          LSAME
C     .. External Subroutines ..
      EXTERNAL          DLARFG, MB04NY
C     .. Intrinsic Functions ..
      INTRINSIC         MIN
C     .. Executable Statements ..
C
C     For efficiency reasons, the parameters are not checked.
C
      IF( MIN( N, P ).EQ.0 )
     $   RETURN
C
      LUPLO = LSAME( UPLO, 'U' )
      IF ( LUPLO ) THEN
C
         DO 10 I = N, 1, -1
C
C           Annihilate the I-th row of A and apply the transformations
C           to the entire block matrix, exploiting its structure.
C
            IM = MIN( N-I+1, P )
            IP = MAX( P-N+I, 1 )
            CALL DLARFG( IM+1, R(I,I), A(I,IP), LDA, TAU(I) )
C
C           Compute
C                                                [ 1 ]
C           w := [ R(1:I-1,I)  A(1:I-1,IP:P) ] * [   ],
C                                                [ v ]
C
C           [ R(1:I-1,I)  A(1:I-1,IP:P) ] =
C           [ R(1:I-1,I)  A(1:I-1,IP:P) ] - tau * w * [ 1 v' ].
C
            IF ( I.GT.0 )
C
     $         CALL MB04NY( I-1, IM, A(I,IP), LDA, TAU(I), R(1,I), LDR,
     $                      A(1,IP), LDA, DWORK )
C
C           Compute
C                                        [ 1 ]
C           w := [ B(:,I)  C(:,IP:P) ] * [   ],
C                                        [ v ]
C
C           [ B(:,I)  C(:,IP:P) ] = [ B(:,I)  C(:,IP:P) ] -
C                                   tau * w * [ 1 v' ].
C
            IF ( M.GT.0 )
     $         CALL MB04NY( M, IM, A(I,IP), LDA, TAU(I), B(1,I), LDB,
     $                      C(1,IP), LDC, DWORK )
   10    CONTINUE
C
      ELSE
C
         DO 20 I = N, 2 , -1
C
C           Annihilate the I-th row of A and apply the transformations
C           to the first block row, exploiting its structure.
C
            CALL DLARFG( P+1, R(I,I), A(I,1), LDA, TAU(I) )
C
C           Compute
C                                             [ 1 ]
C           w := [ R(1:I-1,I)  A(1:I-1,:) ] * [   ],
C                                             [ v ]
C
C           [ R(1:I-1,I)  A(1:I-1,:) ] = [ R(1:I-1,I)  A(1:I-1,:) ] -
C                                        tau * w * [ 1 v' ].
C
            CALL MB04NY( I-1, P, A(I,1), LDA, TAU(I), R(1,I), LDR, A,
     $                   LDA, DWORK )
   20    CONTINUE
C
         CALL DLARFG( P+1, R(1,1), A(1,1), LDA, TAU(1) )
         IF ( M.GT.0 ) THEN
C
C           Apply the transformations to the second block row.
C
            DO 30 I = N, 1, -1
C
C              Compute
C                                   [ 1 ]
C              w := [ B(:,I)  C ] * [   ],
C                                   [ v ]
C
C              [ B(:,I)  C ] = [ B(:,I)  C ] - tau * w * [ 1 v' ].
C
               CALL MB04NY( M, P, A(I,1), LDA, TAU(I), B(1,I), LDB, C,
     $                      LDC, DWORK )
   30       CONTINUE
C
         END IF
      END IF
      RETURN
C *** Last line of MB04ND ***
      END