      SUBROUTINE DGGEVX_F95( A, B, ALPHAR, ALPHAI, BETA, VL, VR, &
     &  BALANC, ILO, IHI, LSCALE, RSCALE, ABNRM, BBNRM, RCONDE, RCONDV, INFO )
!
!  -- LAPACK95 interface driver routine (version 3.0) --
!     UNI-C, Denmark; Univ. of Tennessee, USA; NAG Ltd., UK
!     September, 2000
!
!  .. USE STATEMENTS ..
      USE LA_PRECISION, ONLY: WP => DP
      USE LA_AUXMOD, ONLY: ERINFO, LSAME
      USE F77_LAPACK, ONLY: GGEVX_F77 => LA_GGEVX
!  .. IMPLICIT STATEMENT ..
      IMPLICIT NONE
!  .. SCALAR ARGUMENTS ..
      INTEGER, INTENT(OUT), OPTIONAL :: INFO
      INTEGER, INTENT(OUT), OPTIONAL :: ILO,IHI
      REAL(WP), INTENT(OUT), OPTIONAL :: ABNRM, BBNRM
!  .. ARRAY ARGUMENTS ..
      CHARACTER(LEN=1), INTENT(IN), OPTIONAL :: BALANC
      REAL(WP), INTENT(INOUT) :: A(:,:), B(:,:)
      REAL(WP), INTENT(OUT), OPTIONAL, TARGET :: LSCALE(:), RSCALE(:), &
     &  RCONDE(:), RCONDV(:)
      REAL(WP), INTENT(OUT) :: ALPHAR(:), ALPHAI(:), BETA(:)
      REAL(WP), INTENT(OUT), OPTIONAL, TARGET :: VL(:,:), VR(:,:)
!----------------------------------------------------------------------
!
! Purpose
! =======
!
!      LA_GGEVX computes for a pair of n-by-n real or complex matrices
! (A, B) the generalized eigenvalues in the form of scalar pairs
! (alpha; beta) and, optionally, the left and/or right generalized
! eigenvectors.
!      A generalized eigenvalue of the pair (A; B) is, roughly speaking,
! a scalar of the form lambda = alpha / beta such that the matrix
! A - lambda * B is singular. It is usually represented as the pair
! (alpha, beta), as there is a reasonable interpretation of the case
! beta = 0 (even if alpha = 0).
!      A right generalized eigenvector corresponding to a generalized
! eigenvalue lambda is a vector  v  such that ( A - lambda*B)* v = 0.
! A left generalized eigenvector is a vector u such that
! u^H*(A-lambda*B) = 0, where u^H is the conjugate-transpose of u.
!      The computation is based on the (generalized) real or complex
! Schur form of (A, B). (See LA_GGES for details of this form.)
!      Optionally, LA_GGEVX also computes a balancing transformation
! (to improve the conditioning of the eigenvalues and eigenvectors),
! reciprocal condition numbers for the eigenvalues, and reciprocal
! condition numbers for the right eigenvectors. The balancing
! transformation consists of a permutation of rows and columns and/or a
! scaling of rows and columns.
!
! ==========
!
!    SUBROUTINE LA_GGEVX( A, B, <alpha>, BETA, VL=vl, &
!          VR=vr, BALANC=balanc, ILO=ilo, IHI=ihi, &
!          LSCALE=lscale, RSCALE=rscale, ABNRM=abnrm, &
!          BBNRM=bbnrm, RCONDE=rconde, RCONDV=rcondv, &
!          INFO=info )
!        <type>(<wp>), INTENT(INOUT) :: A(:,:), B(:,:)
!        <type>(<wp>), INTENT(OUT) :: <alpha(:)>, BETA(:)
!        <type>(<wp>), INTENT(OUT), OPTIONAL :: VL(:,:), VR(:,:)
!        CHARACTER(LEN=1), INTENT(IN), OPTIONAL :: BALANC
!        INTEGER, INTENT(OUT), OPTIONAL :: ILO, IHI
!        REAL(<wp>), INTENT(OUT), OPTIONAL :: LSCALE(:),
!             RSCALE(:), RCONDE(:), RCONDV(:)
!        REAL(<wp>), INTENT(OUT), OPTIONAL :: ABNRM, BBNRM
!        INTEGER, INTENT(OUT), OPTIONAL :: INFO
!     where
!        <type>     ::= REAL | COMPLEX
!        <wp>       ::= KIND(1.0) | KIND(1.0D0)
!        <alpha>    ::= ALPHAR, ALPHAI | ALPHA
!        <alpha(:)> ::= ALPHAR(:), ALPHAI(:) | ALPHA(:)
!
!
! Arguments
! =========
!
! A        (input/output) REAL or COMPLEX square array, shape (:, :).
!          On entry, the matrix A.
!          On exit, A has been overwritten. If the left, the right or
!          both generalized eigenvectors are computed, then A contains
! 	   the first part of the real/complex Schur form of the
!          "balanced" versions of the matrix pair (A, B).
! B        (input/output) REAL or COMPLEX square array, shape (:, :)
!          with size(B, 1) = size(A, 1).
!          On entry, the matrix B.
!          On exit, B has been overwritten. If the left, the right or
! 	   both generalized eigenvectors are computed, then B contains
! 	   the second part of the real/complex Schur form of the "bal-
!          anced" versions of the matrix pair (A, B).
! <alpha>  (output) REAL or COMPLEX array, shape (:) with
!          size(<alpha>) = size(A, 1).
!          The values of alpha.
!          <alpha(:)> ::= ALPHAR(:), ALPHAI(:) |  ALPHA(:),
!          where
!          ALPHAR(:), ALPHAI(:) are of REAL type (for the real and
! 	   imaginary parts) and ALPHA(:) is of COMPLEX type.
! BETA     (output) REAL or COMPLEX array, shape (:) with
!          size(BETA) = size(A,1).
!          The values of beta.
!          Note: The generalized eigenvalues of the pair (A, B) are the
! 	   scalars lambda(j) = alpha(j) / beta(j) . These quotients
!          may easily over- or underflow, and beta(j) may even be zero.
! 	   Thus, the user should avoid computing them naively.
!          Note: If A and B are real then complex eigenvalues occur in
! 	   complex conjugate pairs. Each pair is stored consecutively.
! 	   Thus a complex conjugate pair is given by
!                 lambda(j) = (ALPHAR(j) + i*ALPHAI(j))/BETA(j)
!                 lambda(j+1) = (ALPHAR(j+1) + i*ALPHAI(j+1))/BETA(j+1)
!          where
!              ALPHAI(j)/BETA(j)= - (ALPHAI(j+1)/BETA(j+1))
! VL       Optional (output) REAL or COMPLEX square array, shape (:, :)
!          with  size(VL, 1) = size(A, 1).
!         The left generalized eigenvectors u(j) are stored in the
!         columns of VL in the order of their eigenvalues. Each
!         eigenvector is scaled so the largest component has
!             |realpart| + |imag.part| = 1,
!         except that for eigenvalues with alpha = beta = 0, a zero
!         vector is returned as the corresponding eigenvector.
!         Note: If A and B are real then complex eigenvectors, like
!         their eigenvalues, occur in complex conjugate pairs. The real
!         and imaginary parts of the first eigenvector of the pair are
!         stored in VL(:,j) and VL(:,j+1) . Thus a complex conjugate
!         pair is given by
!         u(j) = VL(:,j) + i*VL(:,j+1), u(j+1) = VL(:,j) - i*VL(:,j+1)
! VR      Optional (output) REAL or COMPLEX square array, shape (:,:)
!         with size(VR,1) = size(A,1).
!         The right generalized eigenvectors v(j) are stored in the
!         columns of VR in the order of their eigenvalues. Each
! 	  eigenvector is scaled so the largest component has
! 	  | realpart | + | imag.part | = 1,except that for eigenvalues
! 	  with alpha = beta = 0, a zero vector is returned as the
! 	  corresponding eigenvector.
!         Note: If A and B are real then complex eigenvectors, like
! 	  their eigenvalues, occur in complex conjugate pairs. The real
! 	  and imaginary parts of the first eigenvector of the pair are
! 	  stored in VR(:,j) and VR(:,j+1) . Thus a complex conjugate
! 	  pair is given by
!         v(j) = VR(:,j) + i*VR(:,j+1), v(j+1) = VR(:,j) - i*VR(:,j+1)
! BALANC  Optional (input) CHARACTER(LEN=1).
!         Specifies the balance option to be performed.
!            = 'N': do not permute or scale;
!            = 'P': permute only;
!            = 'S': scale only;
!            = 'B': both permute and scale.
!         Default value: 'N".
!         Note: Computed reciprocal condition numbers will be for the
! 	  matrices after balancing. Permuting does not change condition
! 	  numbers (in exact arithmetic), but scaling does.
! ILO,IHI Optional (output) INTEGER.
!         ILO and IHI are integer values such that on exit A(i,j) = 0
! 	  and B(i,j) = 0 if i > j and j =1,...,ILO-1 or
! 	  i = IHI+1,...,n.
! 	  If BALANC = 'N' or 'S', then ILO = 1 and IHI = n.
! LSCALE  Optional (output) REAL array, shape (:) with size(LSCALE) =
!         size(A, 1).
!         Details of the permutations and scaling factors applied to
! 	  the left side of A and B. If PL(j) is the index of the row
! 	  interchanged with row j, and DL(j) is the scaling factor
!         applied to row j, then
!                 PL(j) = LSCALE(j),  j = 1,...,ILO-1 and IHI+1,..., n
! 	  and
! 	        DL(j) = LSCALE(j),  j = ILO, ..., IHI
! RSCALE  Optional (output) REAL array, shape (:), size(RSCALE) =
!         size(A, 1).
!         Details of the permutations and scaling factors applied to the
! 	  right side of A and B. If PR(j) is the index of the column
! 	  interchanged with column j, and DR(j) is the scaling factor
! 	  applied to column j, then
!                PR(j) = RSCALE(j),  j = 1,...,ILO-1 and IHI+1,..., n
!         and
!                DR(j) = RSCALE(j),  j = ILO, ..., IHI
! ABNRM   Optional (output) REAL.
!         The l1 norm of A after balancing.
! BBNRM   Optional (output) REAL.
!         The l 1 norm of B after balancing.
! RCONDE  Optional (output) REAL array, shape (:) with size(RCONDE) =
!         size(A, 1).
!         The reciprocal condition numbers of the eigenvalues.
! RCONDV  Optional (output) REAL array, shape (:) with size(RCONDE) =
!         size(A, 1).
!         The estimated reciprocal condition numbers of the right
! 	  eigenvectors. If the eigenvalues cannot be reordered to
! 	  compute RCONDV(j) then RCONDV(j) is set to 0. This can only
!         occur when the true value would be very small.
! INFO    Optional (output) INTEGER.
!         = 0: successful exit.
!         < 0: if INFO = -i, the i-th argument had an illegal value.
!         > 0: if INFO = i, and i is
! 	    <= n: The QZ iteration failed. No eigenvectors have been
! 	          calculated, but (alpha(j) , BETA(j) ) should be
! 	   correct for j = INFO + 1,..., n.
!             = n+1: another part of the algorithm failed.
!             = n+2: a failure occurred during the computation of the
! 	          generalized eigenvectors.
!         If INFO is not present and an error occurs, then the program
! 	is terminated with an error message.
!----------------------------------------------------------------------
!  .. LOCAL PARAMETERS ..
      CHARACTER(LEN=8), PARAMETER :: SRNAME = 'LA_GGEVX'
!  .. LOCAL SCALARS ..
      CHARACTER(LEN=1) :: LJOBVL, LJOBVR, LBALANC, LSENSE
      INTEGER, SAVE :: LWORK = 0
      INTEGER :: N, LINFO, LD, ISTAT, S1VL, S2VL, S1VR, S2VR, &
     &  SALPHAR, SALPHAI, SBETA, LILO, LIHI, SRCONDE, SRCONDV, SLSCALE, SRSCALE
      REAL(WP) :: LABNRM, LBBNRM
!  .. LOCAL ARRAYS ..
      REAL(WP), TARGET :: LLVL(1,1), LLVR(1,1), WORKMIN(1)
      REAL(WP), POINTER :: LRCONDE(:), LRCONDV(:), LLSCALE(:), LRSCALE(:)
      REAL(WP), POINTER :: WORK(:)
      INTEGER, POINTER :: IWORK(:)
      LOGICAL, POINTER :: BWORK(:)
!  .. INTRINSIC FUNCTIONS ..
      INTRINSIC MAX, PRESENT, SIZE
!  .. EXECUTABLE STATEMENTS ..
   LINFO = 0; ISTAT = 0; N = SIZE(A,1); LD = MAX(1,N)
   SALPHAR = SIZE(ALPHAR); SALPHAI = SIZE(ALPHAI); SBETA = SIZE(BETA)
   IF( PRESENT(BALANC) )THEN; LBALANC = BALANC; ELSE; LBALANC = 'N'; ENDIF
   IF( PRESENT(LSCALE) )THEN; SLSCALE = SIZE(LSCALE); ELSE; SLSCALE = N; ENDIF
   IF( PRESENT(RSCALE) )THEN; SRSCALE = SIZE(RSCALE); ELSE; SRSCALE = N; ENDIF
   IF( PRESENT(RCONDE) )THEN; SRCONDE = SIZE(RCONDE); ELSE; SRCONDE = N; ENDIF
   IF( PRESENT(RCONDV) )THEN; SRCONDV = SIZE(RCONDV); ELSE; SRCONDV = N; ENDIF
   IF( PRESENT(VL) )THEN; S1VL = SIZE(VL,1); S2VL = SIZE(VL,2); LJOBVL = 'V'
   ELSE; S1VL = 1; S2VL = 1; LJOBVL = 'N'; END IF
   IF( PRESENT(VR) )THEN; S1VR = SIZE(VR,1); S2VR = SIZE(VR,2); LJOBVR = 'V'
   ELSE; S1VR = 1; S2VR = 1; LJOBVR = 'N'; END IF
!  .. TEST THE ARGUMENTS
   IF( N < 0 .OR. SIZE(A,2) /= N )THEN; LINFO = -1
   ELSE IF( SIZE(B,1) /= N .OR. SIZE(B,2) /= N )THEN; LINFO = -2
   ELSE IF( SALPHAR /= N )THEN; LINFO = -3
   ELSE IF( SALPHAI /= N )THEN; LINFO = -4
   ELSE IF( SBETA /= N )THEN; LINFO = -5
   ELSE IF( PRESENT(VL) .AND. ( S1VL /= N .OR. S2VL /= N ) )THEN; LINFO = -6
   ELSE IF( PRESENT(VR) .AND. ( S1VR /= N .OR. S2VR /= N ) )THEN; LINFO = -7
   ELSE IF( .NOT.( LSAME(LBALANC,'N') .OR. LSAME(LBALANC,'P') .OR. &
     &  LSAME(LBALANC,'S') .OR. LSAME(LBALANC,'B') ) )THEN; LINFO = -8
   ELSE IF( SLSCALE /= N )THEN; LINFO = -11
   ELSE IF( SRSCALE /= N )THEN; LINFO = -12
   ELSE IF( SRCONDE /= N )THEN; LINFO = -15
   ELSE IF( SRCONDV /= N )THEN; LINFO = -16
   ELSE IF( N > 0 )THEN
      IF( PRESENT(RCONDE).AND.PRESENT(RCONDV) )THEN; LSENSE = 'B'
      ELSE IF( PRESENT(RCONDE) )THEN; LSENSE = 'E'
      ELSE IF( PRESENT(RCONDV) )THEN; LSENSE = 'V'; ELSE; LSENSE = 'N'; ENDIF

      ALLOCATE(BWORK(N), STAT=ISTAT)
      IF (ISTAT /= 0) THEN; LINFO=-100; GOTO 1000; ENDIF

      ALLOCATE(IWORK(N+6), STAT=ISTAT)
      IF (ISTAT /= 0) THEN; LINFO=-100; GOTO 900; ENDIF

      IF( PRESENT(LSCALE) )THEN; LLSCALE => LSCALE
      ELSE; ALLOCATE( LLSCALE(N), STAT=ISTAT )
        IF (ISTAT /= 0) THEN; LINFO=-100; GOTO 800; ENDIF
      END IF

      IF( PRESENT(RSCALE) )THEN; LRSCALE => RSCALE
      ELSE; ALLOCATE( LRSCALE(N), STAT=ISTAT )
        IF (ISTAT /= 0) THEN; LINFO=-100; GOTO 700; ENDIF
      END IF

      IF( PRESENT(RCONDV) )THEN; LRCONDV => RCONDV
      ELSE; ALLOCATE( LRCONDV(N), STAT=ISTAT )
        IF (ISTAT /= 0) THEN; LINFO=-100; GOTO 600; ENDIF
      END IF

      IF( PRESENT(RCONDE) )THEN; LRCONDE => RCONDE
      ELSE; ALLOCATE( LRCONDE(N), STAT=ISTAT )
        IF (ISTAT /= 0) THEN; LINFO=-100; GOTO 500; ENDIF
      END IF

! .. DETERMINE THE WORKSPACE ..
! .. QUERING THE SIZE OF WORKSPACE ..
      LWORK = -1
      IF (PRESENT (VL)) THEN
        IF (PRESENT (VR)) THEN
           CALL GGEVX_F77( LBALANC, LJOBVL, LJOBVR, LSENSE, N, A, LD, B, LD, ALPHAR, ALPHAI, &
&             BETA, VL, S1VL, VR, S1VR, LILO, LIHI, LLSCALE, LRSCALE, LABNRM, LBBNRM, &
&             LRCONDE, LRCONDV, WORKMIN, LWORK, IWORK, BWORK, LINFO )
        ELSE
	   CALL GGEVX_F77( LBALANC, LJOBVL, LJOBVR, LSENSE, N, A, LD, B, LD, ALPHAR, ALPHAI, &
&             BETA, VL, S1VL, LLVR, S1VR, LILO, LIHI, LLSCALE, LRSCALE, LABNRM, LBBNRM, &
&             LRCONDE, LRCONDV, WORKMIN, LWORK, IWORK, BWORK, LINFO )
        ENDIF
       ELSE
        IF (PRESENT (VR)) THEN
           CALL GGEVX_F77( LBALANC, LJOBVL, LJOBVR, LSENSE, N, A, LD, B, LD, ALPHAR, ALPHAI, &
&             BETA, LLVL, S1VL, VR, S1VR, LILO, LIHI, LLSCALE, LRSCALE, LABNRM, LBBNRM, &
&             LRCONDE, LRCONDV, WORKMIN, LWORK, IWORK, BWORK, LINFO )
        ELSE
	   CALL GGEVX_F77( LBALANC, LJOBVL, LJOBVR, LSENSE, N, A, LD, B, LD, ALPHAR, ALPHAI, &
&             BETA, LLVL, S1VL, LLVR, S1VR, LILO, LIHI, LLSCALE, LRSCALE, LABNRM, LBBNRM, &
&             LRCONDE, LRCONDV, WORKMIN, LWORK, IWORK, BWORK, LINFO )
        ENDIF
       ENDIF
      LWORK = WORKMIN(1)

      ALLOCATE(WORK(LWORK), STAT=ISTAT)
      IF (ISTAT /= 0) THEN; LINFO=-100; GOTO 100; ENDIF
      IF (PRESENT (VL)) THEN
        IF (PRESENT (VR)) THEN
     	    CALL GGEVX_F77( LBALANC, LJOBVL, LJOBVR, LSENSE, N, A, LD, B, LD, ALPHAR, ALPHAI, &
&             BETA, VL, S1VL, VR, S1VR, LILO, LIHI, LLSCALE, LRSCALE, LABNRM, LBBNRM,&
&             LRCONDE, LRCONDV, WORK, LWORK, IWORK, BWORK, LINFO )
        ELSE
	    CALL GGEVX_F77( LBALANC, LJOBVL, LJOBVR, LSENSE, N, A, LD, B, LD, ALPHAR, ALPHAI, &
&             BETA, VL, S1VL, LLVR, S1VR, LILO, LIHI, LLSCALE, LRSCALE, LABNRM, LBBNRM,&
&             LRCONDE, LRCONDV, WORK, LWORK, IWORK, BWORK, LINFO )
        ENDIF
      ELSE
        IF (PRESENT (VR)) THEN
     	    CALL GGEVX_F77( LBALANC, LJOBVL, LJOBVR, LSENSE, N, A, LD, B, LD, ALPHAR, ALPHAI, &
&             BETA, LLVL, S1VL, VR, S1VR, LILO, LIHI, LLSCALE, LRSCALE, LABNRM, LBBNRM,&
&             LRCONDE, LRCONDV, WORK, LWORK, IWORK, BWORK, LINFO )
        ELSE
	    CALL GGEVX_F77( LBALANC, LJOBVL, LJOBVR, LSENSE, N, A, LD, B, LD, ALPHAR, ALPHAI, &
&             BETA, LLVL, S1VL, LLVR, S1VR, LILO, LIHI, LLSCALE, LRSCALE, LABNRM, LBBNRM,&
&             LRCONDE, LRCONDV, WORK, LWORK, IWORK, BWORK, LINFO )
        ENDIF
      ENDIF
      IF( LINFO == 0 ) LWORK = INT(WORK(1)+1)

      IF( PRESENT(ILO) ) ILO = LILO; IF( PRESENT(IHI) ) IHI = LIHI
      IF( PRESENT(ABNRM) ) ABNRM = LABNRM
      IF( PRESENT(BBNRM) ) BBNRM = LBBNRM

      DEALLOCATE(WORK)
100  IF (.NOT. PRESENT(RCONDE)) DEALLOCATE(LRCONDE)
500  IF (.NOT. PRESENT(RCONDV)) DEALLOCATE(LRCONDV)
600  IF (.NOT. PRESENT(RSCALE)) DEALLOCATE(LRSCALE)
700  IF (.NOT. PRESENT(LSCALE)) DEALLOCATE(LLSCALE)
800  DEALLOCATE (IWORK)
900  DEALLOCATE (BWORK)
     ENDIF
1000  CALL ERINFO(LINFO,SRNAME,INFO,ISTAT)
END SUBROUTINE DGGEVX_F95


      SUBROUTINE Q2BCS (EST,PLANAR,RMAT,ET,IERROR)
C
C     BASIC CALCULATIONS ARE PERFORMED FOR THE QDMEM2 ELEMENT IN THIS
C     ROUTINE (SINGLE-PRECISION VERSION)
C
      LOGICAL     PLANAR
      REAL        EST(1)
      REAL        MAG   ,D12(3),G1(3) ,IAREA  ,D13(3) ,GRID(3,5),
     1            G2(3) ,ITWOH ,D24(3),VEC(3) ,G3(3)  ,ET(3,3)  ,
     2            G5(3) ,G4(3) ,SADOTB,RMAT(3,5)
      EQUIVALENCE (GRID(1,1),G1(1)),(GRID(1,2),G2(1)),(GRID(1,3),G3(1)),
     1            (GRID(1,4),G4(1)),(GRID(1,5),G5(1))
C
C     MOVE GRID COORDINATES
C
      DO 10 I = 1,3
      G1(I) = EST(I+10)
      G2(I) = EST(I+14)
      G3(I) = EST(I+18)
      G4(I) = EST(I+22)
   10 CONTINUE
C
C     FORM  D   , D   AND  D   VECTORS
C            13    24       12
C
      DO 20 I = 1,3
      D12(I) = G2(I) - G1(I)
      D13(I) = G3(I) - G1(I)
      D24(I) = G4(I) - G2(I)
   20 CONTINUE
C
C     NVEC  =  D13 CROSS D24 = K-VECTOR (UN-NORMALIZED)
C
      CALL SAXB (D13,D24,VEC)
      MAG   =  SQRT (SADOTB(VEC,VEC))
      IAREA = 0.5*MAG
C
C     NORMALIZE K-VECTOR
C
      IF (MAG) 100,100,30
   30 ET(1,3) = VEC(1)/MAG
      ET(2,3) = VEC(2)/MAG
      ET(3,3) = VEC(3)/MAG
C
C     H = .5 * (D   DOT K-VEC)
C                12
C
      ITWOH = SADOTB(D12,ET(1,3))
C
C     I-VECTOR (UN-NORMALIZED) = (D  ) - 2 H (K-VECTOR)
C                                  12
C
      DO 40 I = 1,3
      VEC(I) = D12(I) - ITWOH*ET(I,3)
   40 CONTINUE
      MAG =  SQRT(SADOTB(VEC,VEC))
C
C     NORMALIZE I-VECTOR
C
      IF (MAG) 100,100,50
   50 ET(1,1) = VEC(1)/MAG
      ET(2,1) = VEC(2)/MAG
      ET(3,1) = VEC(3)/MAG
C
C     JVEC = KVEC CROSS IVEC
C
      CALL SAXB (ET(1,3),ET(1,1),ET(1,2))
C
C     FILL THE SUB-TRIANGLE ELEMENT COORDINATE MATRIX
C
      DO 60 I = 1,3
      G5(I) = 0.25*(G1(I) + G2(I) + G3(I) + G4(I))
   60 CONTINUE
      RMAT(1,1) = 0.0
      RMAT(2,1) = 0.0
      RMAT(3,1) =-ITWOH/2.0
      DO 70 I = 2,5
      VEC(1) = GRID(1,I) - G1(1)
      VEC(2) = GRID(2,I) - G1(2)
      VEC(3) = GRID(3,I) - G1(3)
      CALL GMMATS (ET,3,3,0, VEC,3,1,0, RMAT(1,I))
      RMAT(1,I) = RMAT(1,I) + RMAT(1,1)
      RMAT(2,I) = RMAT(2,I) + RMAT(2,1)
      RMAT(3,I) = RMAT(3,I) + RMAT(3,1)
   70 CONTINUE
C
C     SET PLANAR FLAG .TRUE. OR .FALSE.
C
      IF ((ITWOH/2.0)**2/IAREA .LE. 0.01) GO TO 80
      PLANAR = .FALSE.
      GO TO 90
   80 PLANAR = .TRUE.
C
C     ALL BASIC CALCULATIONS NOW COMPLETE
C
   90 IERROR = 0
      RETURN
C
C     ERROR CONDITION, BAD ELEMENT GEOMETRY.
C
  100 IERROR = 1
      RETURN
      END


	SUBROUTINE GCLGRNF ( kx, ky, grid, ioffx, ioffy, iskip, nlvl,
     +			     clvl, clbl, icolr, lintyp, linwid, linlbl,
     +			     scflag, zgrid, jhline, jvline, iret )
C************************************************************************
C* GCLGRNF								*
C* 									*
C* This subroutine draws contours through a grid of data.  The 		*
C* algorithm used is based on a two-dimensional Lagrangian fitting of	*
C* the grid points.  It is the original GEMPAK contouring program.	*
C* The original subroutine, GCONTR, had a different calling sequence.	*
C* 									*
C* GCLGRNF ( KX, KY, GRID, IOFFX, IOFFY, ISKIP, NLVL, CLVL, CLBL, 	*
C*           ICOLR, LINTYP, LINWID, LINLBL, SCFLAG, ZGRID, JHLINE,      *
C*           JVLINE, IRET )						*
C*									*
C* Input parameters:							*
C*									*
C*	KX		INTEGER		Number of x grid points 	*
C*	KY		INTEGER		Number of y grid points		*
C*	GRID (KX,KY)	REAL		Grid data array			*
C*	IOFFX		INTEGER		X offset to first point		*
C*	IOFFY		INTEGER		Y offset to first point		*
C*	ISKIP		INTEGER		Skip factor in original grid	*
C*	NLVL		INTEGER		Number of contour levels	*
C*	CLVL   (NLVL)	REAL		Contour level values		*
C*	CLBL   (NLVL)	CHAR*		Contour labels			*
C*	ICOLR  (NLVL)	INTEGER		Contour color numbers		*
C*	LINTYP (NLVL)	INTEGER		Contour line types		*
C*	LINWID (NLVL)	INTEGER 	Contour line widths		*
C*	LINLBL (NLVL)	INTEGER		Contour label types		*
C*	SCFLAG		LOGICAL		Small contour suppress flag	*
C*									*
C* Work parameters:							*
C*      ZGRID (KX*KY)   REAL            Array for grid points           *
C*      JHLINE (KX*KY)  INTEGER         Array for Horiz crossings       *
C*      JVLINE (KX*KY)  INTEGER         Array for Vert crossings       *
C*									*
C* Output parameters:							*
C*									*
C* 	IRET		INTEGER		Return code			*
C**									*
C* Log:									*
C* M. desJardins/NMC	11/91	From GCONTR				*
C* S. Jacobs/NCEP	 1/96	Added CADJST to adjust grid values	*
C* M. desJardins/NCEP	 9/98	Eliminate subboxing when CONTUR=1	*
C* C. Bailey/HPC	 6/06	Added contour label array		*
C* C. Bailey/HPC	10/06	Added small contour suppress flag	*
C* S. Gilbert/NCEP	07/07	Added Work arrays as input args		*
C************************************************************************
	INCLUDE		'GEMPRM.PRM'
	INCLUDE 	'ERROR.PRM'
	INCLUDE		'CONTUR.CMN'
C*
	REAL		grid (*), clvl (*), zgrid (*)
	INTEGER		icolr (*), lintyp (*), linwid (*), linlbl (*)
	INTEGER		jhline (*), jvline (*)
	CHARACTER*(*)	clbl (*)
	LOGICAL		scflag
C------------------------------------------------------------------------
	iret = NORMAL
C
C*	Read in the box subset factor.  Check that it is valid and set
C*	it in common for the rest of the original contouring code.
C
	IF  ( jbxsub .lt. 2 )  THEN
	    numsub = 2
	  ELSE IF  ( jbxsub .gt. 48 )  THEN
	    numsub = 50
	  ELSE
	    numsub = jbxsub + 2
	END IF
C
C*	Move offsets and skip factor into common.
C
	offx = FLOAT ( ioffx )
	offy = FLOAT ( ioffy )
	skip = FLOAT ( iskip )
C
C*	This program expects the data in the zgrid array
C
	DO  i = 1, kx * ky
	    zgrid (i) = grid (i)
	END DO
C
C*	Adjust the grid values.
C
	CALL CADJST ( kx, ky, nlvl, clvl, zgrid, ier )
C
C*	Save grid size as ISIZE, JSIZE.
C
	isize = kx
	jsize = ky
C
C*	Compute distance along each subbox.
C
	fincxy = 1. / FLOAT ( numsub - 1 )
C
C*	Call the driver for the Lagrangian contouring.
C
	CALL CLDRIV  ( nlvl, clvl, clbl, icolr, lintyp, linwid,
     +		       linlbl, scflag, zgrid, jhline, jvline, ier )
C*
	RETURN
	END


      double complex function trI1(m1,mu2,ep)
c--- this is a switchyard routine: depending on value of
c--- integer variable, this routine either:
c---  TRscalarselect = 1    calls QCDLoop for scalar integral
c---  TRscalarselect = 2    calls OneLOop for scalar integral
c---  TRscalarselect = 3    calls both routines and compares results
c      use avh_olo
      implicit none
      include 'TRscalarselect.f'
      double precision m1,mu2
      double complex qlI1,resQCDLoop,resOneLOop,result(0:2)
      integer ep

      TRscalarselect=1

c--- call to QCDLoop if necessary
      if ((TRscalarselect .eq. 1) .or.(TRscalarselect .eq. 3)) then
        resQCDLoop=qlI1(m1,mu2,ep)
      endif

      if (TRscalarselect .eq. 1) then
        trI1=resQCDLoop
        return
      endif

c--- initializations should be moved elsewhere and called only once
      call olo_onshell(1d-8)
      call olo_unit(-1,'error')

      call olo_a0(result,m1,dsqrt(mu2))
      resOneLOop=result(abs(ep))
      trI1=resOneLOop

      if (TRscalarselect .eq. 3) then
        if ((cdabs(resOneLOop) .gt. 1d-12) .and.
     &      (abs(resQCDLoop/resOneLOop-1d0) .gt. 1d-12)) then
          write(6,*) 'trI1: ',m1,ep
          write(6,*) 'QCDLoop:',resQCDLoop
          write(6,*) 'OneLOop:',resOneLOop
          write(6,*) '->ratio:',resQCDLoop/resOneLOop
        endif
      endif

      return
      end


!########################################################
!     Program  : HMMPT
!     TYPE     : Main program
!     PURPOSE  : Solve the Hubbard model using DMFT away from half-filling
! with modified itertive perturbation scheme (MPT)
!     AUTHORS  : Adriano Amaricci
!########################################################
MODULE COMMON
  USE GREENFUNX
  USE BROYDEN
  implicit none
  real(8)                :: xmu0(2),n(2),n0(2)
  type(matsubara_gf)     :: fg(2),fg0(2)
  complex(8),allocatable :: sold(:,:),sigma(:,:),gamma(:,:)
END MODULE COMMON

FUNCTION FUNCV(x)
  USE COMMON
  USE DMFT_IPT
  implicit none
  real(8),dimension(:),intent(in)  ::  x
  real(8),dimension(size(x))       ::  funcv
  xmu0(1)=x(1)
  xmu0(2)=x(2)
  !Hartree corrected WF is: \tilde{\calG0}^-1 = \calG0^-1 +xmu -xmu0 -U*n
  !####CHECK THIS#####
  fg0(1)%iw = one/(one/gamma(1,:) +xmu-xmu0(1)-u*(n(2)-0.5d0))
  fg0(2)%iw = one/(one/gamma(2,:) +xmu-xmu0(2)-u*(n(1)-0.5d0))

  call fftgf_iw2tau(fg0(1)%iw,fg0(1)%tau,beta)
  call fftgf_iw2tau(fg0(2)%iw,fg0(2)%tau,beta)

  n0(1)=-real(fg0(1)%tau(L))
  n0(2)=-real(fg0(2)%tau(L))
  funcv(1)=n(1)-n0(1)
  funcv(2)=n(2)-n0(2)
  write(*,"(6(f13.9))")n(1),n(2),n0(1),n0(2),xmu0(1),xmu0(2)
END FUNCTION FUNCV

program hmmpt_matsubara
  USE DMFT_IPT
  USE COMMON
  USE IOTOOLS
  implicit none
  real(8)    :: x(2),z(2),extH
  logical    :: check
  complex(8) :: zeta(2),zita
  logical :: converged
  integer :: i
  real(8),dimension(:),allocatable :: wm

  call read_input("inputIPT.in")
  call allocate_gf(fg,L)
  call allocate_gf(fg0,L)

  allocate(sigma(2,L))
  allocate(gamma(2,L))
  allocate(sold(2,L))
  allocate(wm(L))

  wm = pi/beta*real(2*arange(1,L)-1,8)

  n=0.5d0 ; xmu0=xmu
  sigma(1,:)= U*(n(1)-0.5d0)
  sigma(2,:)= U*(n(2)-0.5d0)
  iloop=0 ; converged=.false.;D=1.d0
  extH=0.1d0
  do while (.not.converged)
     iloop=iloop+1
     write(*,"(A,i5)")"DMFT-loop",iloop
     do i=1,L
        zeta(1)    = xi*wm(i) + xmu +extH - sigma(1,i)
        zeta(2)    = xi*wm(i) + xmu -extH - sigma(2,i)
        zita=sqrt(zeta(1)*zeta(2))
        fg(1)%iw(i) = gfbethe(wm(i),zita,D)
        fg(2)%iw(i) = gfbethe(wm(i),zita,D)
     enddo
     fg(1)%iw(i) =  fg(1)%iw(i)*sqrt(zeta(2)/zeta(1))
     fg(2)%iw(i) =  fg(2)%iw(i)*sqrt(zeta(1)/zeta(2))
     call fftgf_iw2tau(fg(1)%iw,fg(1)%tau,beta)
     call fftgf_iw2tau(fg(2)%iw,fg(2)%tau,beta)
     n(1)=-real(fg(1)%tau(L))
     n(2)=-real(fg(2)%tau(L))
     extH=0.d0

     !Get the regular Weiss Field: \Gamma(w+xmu)
     gamma(1,:)= one/(one/fg(1)%iw + sigma(1,:))
     gamma(2,:)= one/(one/fg(2)%iw + sigma(2,:))

     !Fix the xmu0 w/ condition n0=n
     x(:)=xmu
     call broydn(x,check)
     xmu0=x

     gamma(1,:)=fg0(1)%iw ; gamma(2,:)=fg0(2)%iw
     sigma = solve_mpt_af_matsubara(gamma,n,n0,xmu0)
     sigma = weight*sigma + (1.d0-weight)*sold
     sold=sigma
     converged=check_convergence(sigma,eps_error,Nsuccess,Nloop)
     ! z=1.d0 - dimag(sigma%iw(1))/wm(1);z=1.d0/z
     call splot("nVSiloop.ipt",iloop,n(1),n(2),append=TT)
     ! call splot("zetaVSiloop.ipt",iloop,z,append=TT)
  enddo
  call close_file("nVSiloop.ipt")
  ! call close_file("zetaVSiloop.ipt")
  call splot("G_iw.ipt",wm,fg(1)%iw,append=printf)
  call splot("G_iw.ipt",wm,fg(2)%iw,append=TT)
  call splot("G0_iw.ipt",wm,gamma(1,:),append=printf)
  call splot("G0_iw.ipt",wm,gamma(2,:),append=TT)
  call splot("Sigma_iw.ipt",wm,sigma(1,:),append=printf)
  call splot("Sigma_iw.ipt",wm,sigma(2,:),append=TT)
  stop
end program hmmpt_matsubara


! Copyright (c) 2019 Etienne Descamps
! All rights reserved.
!
! Redistribution and use in source and binary forms, with or without modification,
! are permitted provided that the following conditions are met:
!
! 1. Redistributions of source code must retain the above copyright notice,
!    this list of conditions and the following disclaimer.
!
! 2. Redistributions in binary form must reproduce the above copyright notice,
!    this list of conditions and the following disclaimer in the documentation and/or
!    other materials provided with the distribution.
!
! 3. Neither the name of the copyright holder nor the names of its contributors may be
!    used to endorse or promote products derived from this software without specific prior
!    written permission.
!
! THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
! ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
! WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
! IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
! INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
! (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
! LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
! THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
! NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
! EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

Module mlf_normal_dist
  Use ieee_arithmetic
  Use iso_c_binding
  Use mlf_utils
  Use mlf_cfuns
  Use mlf_distribution
  IMPLICIT NONE
  PRIVATE

  Type, Public, Extends(mlf_distributionWithQuantile_type) :: mlf_normal_distribution
    real(c_double) :: mu, sigma
  Contains
    procedure :: getParameters => Normal_getParameters
    procedure :: withParameters => Normal_withParameters
    procedure :: getStats => Normal_getStats
    procedure :: fitWithData => Normal_fitWithData
    procedure :: quantile => Normal_quantile
    procedure :: computeCDF => Normal_computeCDF
    procedure :: computePDF => Normal_computePDF
    procedure :: computeLogPDF => Normal_computeLogPDF
  End Type mlf_normal_distribution

  Type, Public, Extends(mlf_normal_distribution) :: mlf_logNormal_distribution
  Contains
    procedure :: getStats => logNormal_getStats
    procedure :: fitWithData => logNormal_fitWithData
    procedure :: quantile => logNormal_quantile
    procedure :: computeCDF => logNormal_computeCDF
    procedure :: computePDF => logNormal_computePDF
    procedure :: computeLogPDF => logNormal_computeLogPDF
  End Type mlf_logNormal_distribution
Contains
  Integer Function Normal_getParameters(this, X) Result(info)
    class(mlf_normal_distribution), intent(in) :: this
    real(c_double), intent(out) :: X(:)
    info = 0
    X = [this%mu, this%sigma]
  End Function Normal_getParameters

  Integer Function Normal_withParameters(this, X) Result(info)
    class(mlf_normal_distribution), intent(inout) :: this
    real(c_double), intent(in) :: X(:)
    info = 0
    this%mu = X(1); this%sigma = X(2)
  End Function Normal_withParameters

  Real(c_double) Function Normal_getStats(this, statType) Result(y)
    class(mlf_normal_distribution), intent(in) :: this
    integer, intent(in) :: statType
    ASSOCIATE(mu => this%mu, sigma => this%sigma)
      Select Case(statType)
      Case (mlf_dist_mode, mlf_dist_median, mlf_dist_mean)
        y = mu
      Case Default
        y = IEEE_VALUE(y, IEEE_QUIET_NAN)
      End Select
    END ASSOCIATE
  End Function Normal_getStats

  Integer Function Normal_fitWithData(this, Points, W, prior) Result(info)
    class(mlf_normal_distribution), intent(inout) :: this
    real(c_double), intent(in) :: Points(:)
    real(c_double), intent(in), optional :: W(:)
    class(mlf_distribution_abstract), optional, intent(in) :: prior
    If(PRESENT(W)) Then
      this%mu = SUM(W*Points)/SUM(W)
      this%sigma = SQRT(SUM(W*(Points-this%mu)**2)/(1/SUM(W)-SUM(W*W)*SUM(W)))
    Else
      this%mu = SUM(Points)/SIZE(Points)
      this%sigma = SQRT(SUM((Points-this%mu)**2)/(SIZE(Points)-1d0))
    Endif
    info = 0
  End Function Normal_fitWithData

  Real(c_double) Function Normal_computeCDF(this, x) Result(y)
    class(mlf_normal_distribution), intent(in) :: this
    real(c_double), intent(in) :: x
    y = 0.5d0*(1+c_erf((x-this%mu)/(this%sigma*SQRT(2d0))))
  End Function Normal_computeCDF

  Elemental Real(c_double) Function Normal_computePDF(this, x) Result(y)
    class(mlf_normal_distribution), intent(in) :: this
    real(c_double), intent(in) :: x
    y = 1d0/SQRT(2d0*mlf_PI*this%sigma**2)*EXP(-(x-this%mu)**2/(2*this%sigma**2))
  End Function Normal_computePDF

  Elemental Real(c_double) Function Normal_computeLogPDF(this, x) Result(y)
    class(mlf_normal_distribution), intent(in) :: this
    real(c_double), intent(in) :: x
    y = -0.5d0*LOG(2d0*mlf_PI*this%sigma**2)-(x-this%mu)**2/(2*this%sigma**2)
  End Function Normal_computeLogPDF

  Real(c_double) Function Normal_quantile(this, y) Result(x)
    class(mlf_normal_distribution), intent(in) :: this
    real(c_double), intent(in) :: y
    x = this%mu - this%sigma*SQRT(2d0)*c_erf_inv(2*(1-y))
  End Function Normal_quantile

  Real(c_double) Function logNormal_getStats(this, statType) Result(y)
    class(mlf_logNormal_distribution), intent(in) :: this
    integer, intent(in) :: statType
    ASSOCIATE(mu => this%mu, sigma => this%sigma)
      Select Case(statType)
      Case (mlf_dist_mode)
        y = EXP(mu-sigma**2)
      Case (mlf_dist_median)
        y = EXP(mu)
      Case (mlf_dist_mean)
        y = EXP(mu+0.5d0*sigma**2)
      Case Default
        y = IEEE_VALUE(y, IEEE_QUIET_NAN)
      End Select
    END ASSOCIATE
  End Function logNormal_getStats

  Real(c_double) Function logNormal_quantile(this, y) Result(x)
    class(mlf_logNormal_distribution), intent(in) :: this
    real(c_double), intent(in) :: y
    x = EXP(this%mu - this%sigma*SQRT(2d0)*c_erf_inv(2*(1-y)))
  End Function logNormal_quantile

  Integer Function logNormal_fitWithData(this, Points, W, prior) Result(info)
    class(mlf_logNormal_distribution), intent(inout) :: this
    real(c_double), intent(in) :: Points(:)
    real(c_double), intent(in), optional :: W(:)
    class(mlf_distribution_abstract), optional, intent(in) :: prior
    info = Normal_fitWithData(this, LOG(Points), W)
  End Function logNormal_fitWithData

  Real(c_double) Function logNormal_computeCDF(this, x) Result(y)
    class(mlf_logNormal_distribution), intent(in) :: this
    real(c_double), intent(in) :: x
    y = 0.5d0*(1+c_erf((LOG(x)-this%mu)/(this%sigma*SQRT(2d0))))
  End Function logNormal_computeCDF

  Elemental Real(c_double) Function logNormal_computePDF(this, x) Result(y)
    class(mlf_logNormal_distribution), intent(in) :: this
    real(c_double), intent(in) :: x
    y = 1d0/(x*this%sigma*SQRT(2d0*mlf_PI))*EXP(-(LOG(x)-this%mu)**2/(2*this%sigma**2))
  End Function logNormal_computePDF

  Elemental Real(c_double) Function logNormal_computeLogPDF(this, x) Result(y)
    class(mlf_logNormal_distribution), intent(in) :: this
    real(c_double), intent(in) :: x
    y = -LOG(x)-LOG(this%sigma)-0.5d0*LOG(2d0*mlf_PI)-(LOG(x)-this%mu)**2/(2*this%sigma**2)
  End Function logNormal_computeLogPDF
End Module mlf_normal_dist


program name
    !This program
        !Created by: Enrique Hurtado
        !Date: 21 December 2020
        !Latest update: 21 December 2020
        !History:
            !Date: 12/21/20 || Mod: Program written || By: Enrique Hurtado
        !Purpose: Gaussian fit
            !Specification:
        !Notes:
    !!Code Begins!!

    use setup
    !use mylib
    implicit none
    integer :: i, iter , itmin
    real (dp) :: xstart (1: npar ), fstart , stepi , epsx , epsf, ii, jj
    real (dp), external :: least2

    open(unit = 2, file ='test.dat')
    open(unit = 3, file ='test.d')
    do i = 1, nsp
        read(2 ,*) ii, jj
        write (3, *) ii, jj
        xx(i)= ii
        yy(i)= jj
    end do
    close (unit = 2)
    close (unit = 3)

    minsp = 75
    maxsp = 311
    xstart (1: npar ) = (/ 1.0, 7., 5000. , 0.2 , 0.3  /)
    !minsp = 1
    !maxsp = 16
    !xstart (1: npar ) = (/ 0., 0.09, 1. , 0.00038 , 0.00004  /)
    ifcal = 0
    iprint = 1
    fstart = least2 ( xstart (1: npar ))
    !stop
    itmin = 200
    iter = 2000
    epsf = 0.001_dp
    stepi = 0.1_dp
    iprint = 0

    minsp = 120
    maxsp = 200
    !print *, xstart
    call downhill (npar , least2 , xstart , fstart , &
                    stepi , epsf , itmin , iter )
    !print *, xstart
    iprint = 2
    fstart = least2 ( xstart (1: npar ))
    !print *, ifcal
    print *, xstart, fstart

end program name

function least2(par) result(ss)

    use setup
    implicit none
    real (dp) :: par(npar), a, b, y1, x1, sig1, ss, fi, step, x
    integer :: i, n

    ifcal = ifcal +1
    a = par (1); b = par (2)
    y1 = par(3); x1 = par(4); sig1 = par(5)

    ss = 0._dp
    do i = minsp , maxsp
        fi = a*xx(i) + b + y1*exp( -((xx(i)-x1 )/ sig1 )**2)
        ss = ss + 1./sqrt(yy(i)+2._dp) * (yy(i)-fi )**2
        !print*, i, fi, ss, xx(i), yy(i)
    end do
    ss = ss/abs(maxsp - minsp )
    !print *, ss
    !print *,ifcal ,par (1: npar ),ss
    !print '(i5 ,5 f12.4, f20 .5) ', ifcal ,par (1: npar ),ss
    write (8 ,*) ifcal, ss, ss

    n = 10000
    step = (xx(maxsp)-xx(minsp))/n
    x = xx(minsp)
    if ( iprint /= 0 ) then
        do i = 1 , n
            fi = a*x + b + y1*exp( -((x-x1 )/ sig1 )**2)
            write (unit =iprint ,fmt=*) x, fi
            x = x + step
        end do
    end if

end function least2


C
C   gfortran griddata.f -L/usr/local/gr/lib -lGR
C
      program griddata

      integer GKS_K_MARKERTYPE_SOLID_CIRCLE
      parameter (GKS_K_MARKERTYPE_SOLID_CIRCLE = -1)

      double precision xd(100), yd(100), zd(100)
      double precision x(200), y(200), z(200, 200)
      double precision h(20)
      integer i

      call srand(0)
      do 1 i = 1, 100
          xd(i) = -2 + 4 * rand()
          yd(i) = -2 + 4 * rand()
          zd(i) = xd(i) * exp(-xd(i)*xd(i) - yd(i)*yd(i))
   1  continue

      call gr_setviewport(0.1D0, 0.95D0, 0.1D0, 0.95D0)
      call gr_setwindow(-2.0D0, 2.0D0, -2.0D0, 2.0D0)
      call gr_setspace(-0.5D0, 0.5D0, 0, 90)
      call gr_setmarkersize(1.0D0)
      call gr_setmarkertype(GKS_K_MARKERTYPE_SOLID_CIRCLE)
      call gr_setcharheight(0.024D0)
      call gr_settextalign(2, 0)
      call gr_settextfontprec(3, 0)

      call gr_gridit(100, xd, yd, zd, 200, 200, x, y, z)
      do 2 i = 1, 20
        h(i) = -0.5 + i / 19.0
   2  continue
      call gr_surface(200, 200, x, y, z, 5)
      call gr_contour(200, 200, 20, x, y, h, z, 0)
      call gr_polymarker(100, xd, yd)
      call gr_axes(0.25D0, 0.25D0, -2D0, -2D0, 2, 2, 0.01D0)

      call gr_updatews()
      end


      PROGRAM DSEX01D
C
      PARAMETER (NUM=1000, NX=21, NY=21, NZ=21)
C
C Define the error file, the Fortran unit number, the workstation type,
C and the workstation ID to be used in calls to GKS routines.
C
C     PARAMETER (IERRF=6, LUNIT=2, IWTYPE=1,  IWKID=1)   ! NCGM
C     PARAMETER (IERRF=6, LUNIT=2, IWTYPE=8,  IWKID=1)   ! X Windows
C     PARAMETER (IERRF=6, LUNIT=2, IWTYPE=11, IWKID=1)   ! PDF
C     PARAMETER (IERRF=6, LUNIT=2, IWTYPE=20, IWKID=1)   ! PostScript
C
      PARAMETER (IERRF=6, LUNIT=2, IWTYPE=1,  IWKID=1)
C
      DOUBLE PRECISION XI(NUM), YI(NUM), ZI(NUM), U(NUM)
      DOUBLE PRECISION XO(NX),  YO(NY),  ZO(NZ),  OUTPUT(NX,NY,NZ)
      DOUBLE PRECISION XMIN,    YMIN,    ZMIN
      DOUBLE PRECISION XMAX,    YMAX,    ZMAX
      REAL             XP(NX),  YP(NY),  ZP(NZ),  OUTP(NX,NY,NZ)
      INTEGER I, J, K, IER
C
      DATA XMIN,YMIN,ZMIN/ -2.D0, -2.D0, -2.D0/
      DATA XMAX,YMAX,ZMAX/  2.D0,  2.D0,  2.D0/
C
C  Create random data in three space and define a function.
C  To get this to work on your system, you may have to insert
C  the correct random number generator for your compiler.
C
      DO 10 I=1,NUM
        XI(I) = XMIN+(XMAX-XMIN)*DBLE(DSRND1D())
        YI(I) = YMIN+(YMAX-YMIN)*DBLE(DSRND1D())
        ZI(I) = ZMIN+(ZMAX-ZMIN)*DBLE(DSRND1D())
         U(I) = XI(I)**2 + YI(I)**2 + ZI(I)**2
   10 CONTINUE
C
C  Create the output grid.
C
      DO 102 I=1,NX
        XO(I) = XMIN+(DBLE(I-1)/DBLE(NX-1))*(XMAX-XMIN)
  102 CONTINUE
C
      DO 103 J =1,NY
        YO(J)= YMIN+(DBLE(J-1)/DBLE(NY-1))*(YMAX-YMIN)
  103 CONTINUE
C
      DO 104 K=1,NZ
        ZO(K) = ZMIN+(DBLE(K-1)/DBLE(NZ-1))*(ZMAX-ZMIN)
  104 CONTINUE
C
C  Interpolate.
C
      CALL DSGRID3D(NUM,XI,YI,ZI,U,NX,NY,NZ,XO,YO,ZO,OUTPUT,IER)
      IF (IER .NE. 0) THEN
        WRITE(6,520) IER
  520   FORMAT(' Error ',I3,' returned from DSGRID3D')
        STOP
      ENDIF
C
C  Pass single precision arrays for plotting an isosurface.
C
      DO 105 I=1,NX
        XP(I) = XO(I)
        DO 106 J=1,NY
          YP(J) = YO(J)
          DO 107 K=1,NZ
            ZP(K) = ZO(K)
            OUTP(I,J,K) = OUTPUT(I,J,K)
  107     CONTINUE
  106   CONTINUE
  105 CONTINUE
C
C  Draw plot.
C
      CALL GOPKS (IERRF, ISZDM)
      CALL GOPWK (IWKID, LUNIT, IWTYPE)
      CALL GACWK (IWKID)
      CALL TDEZ3D(NX, NY, NZ, XP, YP, ZP, OUTP, 3.0, 0., 0., 0., 6)
      CALL FRAME
      CALL GDAWK (IWKID)
      CALL GCLWK (IWKID)
      CALL GCLKS
C
      STOP
      END
      REAL FUNCTION DSRND1D()
C
C  Portable random number generator.
C
      PARAMETER (MPLIER=16807,MODLUS=2147483647,MOBYMP=127773,
     +           MOMDMP=2836)
C
      INTEGER HVLUE, LVLUE, TESTV, NEXTN
      SAVE    NEXTN
      DATA JSEED,IFRST/123456789,0/
C
      IF (IFRST .EQ. 0) THEN
        NEXTN = JSEED
        IFRST = 1
      ENDIF
C
      HVLUE = NEXTN / MOBYMP
      LVLUE = MOD(NEXTN, MOBYMP)
      TESTV = MPLIER*LVLUE - MOMDMP*HVLUE
      IF (TESTV .GT. 0) THEN
        NEXTN = TESTV
      ELSE
        NEXTN = TESTV + MODLUS
      ENDIF
      DSRND1D = REAL(NEXTN)/REAL(MODLUS)
C
      RETURN
      END


subroutine curve_jacobian_wrap(s, sd, t, k, nctl, n, nd, vals, row_ptr, col_ind)
  use precision
  implicit none
  ! Input
  real(kind=realType)  , intent(in)      :: t(nctl+k), s(n), sd(nd)
  integer              , intent(in)      :: k, nctl, n, nd
  ! Output
  real(kind=realType)  , intent(inout)   :: vals((n+nd)*k)
  integer              , intent(inout)   :: row_ptr(n+nd+1)
  integer              , intent(inout)   :: col_ind((n+nd)*k)
  real(kind=realType)                   :: basisu(k), basisud(k,k)
  integer                               :: i, j, counter, ileft

  counter = 1
  do i=1, n ! Do the values first
     call findSpan(s(i), k, t, nctl, ileft)
     call basis(t, nctl, k, s(i), ileft, basisu)

     row_ptr(i) = counter-1
     do j=1, k
        col_ind(counter) = ileft-k+j-1
        vals(counter) = basisu(j)
        counter = counter + 1
     end do
  end do
  do i=1, nd ! Do the derivatives next
     call findSpan(sd(i), k, t, nctl, ileft)
     call derivBasis(t, nctl, k, sd(i), ileft, 1, basisud)

     row_ptr(i+n) = counter-1
     do j=1, k
        col_ind(counter) = ileft-k+j-1
        vals(counter) = basisud(2, j)
        counter = counter + 1
     end do
  end do
  row_ptr(n+nd+1) = counter-1
end subroutine curve_jacobian_wrap

subroutine constr_jac(A_val, A_row_ptr, A_col_ind, B_val, B_row_ptr, B_col_ind, C_val, C_row_ptr, C_col_ind, &
     Am, An, Cm, Annz, Bnnz, Cnnz, J_val, J_col_ind, J_row_ptr)
  use precision
  implicit none
  ! Input
  integer         , intent(in)   :: Am, An, Cm, Annz, Bnnz, Cnnz
  real(kind=realType), intent(in)   :: A_val(Annz), B_val(Bnnz), C_val(Cnnz)
  integer         , intent(in)   :: A_col_ind(Annz), B_col_ind(Bnnz), C_col_ind(Cnnz)
  integer         , intent(in)   :: A_row_ptr(Am+1), B_row_ptr(Am+1), C_row_ptr(Cm+1)

  ! Output
  real(kind=realType), intent(out)  :: J_val(Annz+Bnnz+Cnnz)
  integer         , intent(out)  :: J_col_ind(Annz+Bnnz+Cnnz)
  integer         , intent(out)  :: J_row_ptr(Am+Cm+1)

  ! Local
  integer                        :: i, j, counter
  ! This functions assembes the following CSR matrix:
  ! J = [A    B]
  !     [C    0]

  ! Now assmeble the full jacobain
  counter = 1
  J_row_ptr(1) = 0
  do i =1, Am
     do j=1, A_row_ptr(i+1)-A_row_ptr(i)
        J_val(counter) =     A_val(A_row_ptr(i)+j)
        J_col_ind(counter) = A_col_ind(A_row_ptr(i)+j)
        counter = counter + 1
     end do
     do j=1, B_row_ptr(i+1)-B_row_ptr(i)
        J_val(counter) = B_val(B_row_ptr(i)+j)
        J_col_ind(counter) = B_col_ind(B_row_ptr(i)+j) + An
        counter = counter + 1
     end do
     J_row_ptr(i+1) = counter - 1
  end do
  do i =1, Cm
     do j=1, C_row_ptr(i+1)-C_row_ptr(i)
        J_val(counter) = C_val(C_row_ptr(i)+j)
        J_col_ind(counter) = C_col_ind(C_row_ptr(i)+j)
        counter = counter + 1
     end do
     J_row_ptr(i+1+am) = counter - 1
  end do
end subroutine constr_jac

subroutine poly_length(X, n, ndim, length)
  ! Compute the length of the spatial polygon
  use precision
  implicit none

  !Input
  real(kind=realType) , intent(in)    :: X(ndim, n)
  integer             , intent(in)    :: n, ndim

  ! Ouput
  real(kind=realType), intent(out)   :: length

  ! Working
  integer                         :: i, idim
  real(kind=realType)             :: dist

  length = 0.0
  do i=1, n-1
     dist = 0.0
     do idim=1, ndim
        dist = dist + (X(idim, i)-X(idim, i+1))**2
     end do
     length = length + sqrt(dist)
  end do

end subroutine poly_length

subroutine curve_para_corr(t, k, s, coef, nctl, ndim, length, n, X)

  ! Do Hoschek parameter correction
  use precision
  implicit none
  ! Input/Output
  integer              , intent(in)      :: k, nctl, ndim, n
  real(kind=realType)  , intent(in)      :: t(nctl+k)
  real(kind=realType)  , intent(inout)   :: s(n)
  real(kind=realType)  , intent(in)      :: coef(ndim, nctl)
  real(kind=realType)  , intent(in)      :: X(ndim, n)
  real(kind=realType)  , intent(in)      :: length
  ! Working
  integer                               :: i, j, max_inner_iter
  real(kind=realType)                   :: D(ndim), D2(ndim)
  real(kind=realType)                   :: val(ndim), deriv(ndim)
  real(kind=realType)                   :: c, s_tilde, norm

  max_inner_iter = 10
  do i=2, n-1
     call eval_curve(s(i), t, k, coef, nctl, ndim, 1, val)
     call eval_curve_deriv(s(i), t, k, coef, nctl, ndim, deriv)

     D = X(:, i)-val
     c = dot_product(D, deriv)/norm(deriv, ndim)

     inner_loop: do j=1, max_inner_iter

        s_tilde = s(i)+ c*(t(nctl+k)-t(1))/length
        call eval_curve(s_tilde, t, k, coef, nctl, ndim, 1, val)
        D2 = X(:, i)-val
        if (norm(D, ndim) .ge. norm(D2, ndim)) then
           s(i) = s_tilde
           exit inner_loop
        else
           c = c*0.5
        end if
     end do inner_loop
  end do

end subroutine curve_para_corr

function norm(X, n)
  ! Compute the L2 nomr of X
  use precision
  implicit none
  integer, intent(in) :: n
  real(kind=realType), intent(in) :: X(n)

  real(kind=realType) :: norm

  integer            :: i
  norm = 0.0
  do i=1, n
     norm = norm + X(i)**2
  end do
  norm = sqrt(norm)
end function norm

function compute_rms_curve(t, k, s, coef, nctl, ndim, n, X)
  ! Compute the rms
  use precision
  implicit none
  ! Input/Output
  real(kind=realType)  , intent(in)      :: t(k+nctl)
  real(kind=realType)  , intent(in)      :: s(n)
  real(kind=realType)  , intent(in)      :: coef(ndim, nctl)
  integer           , intent(in)      :: k, nctl, ndim, n
  real(kind=realType)  , intent(in)      :: X(ndim, n)
  real(kind=realType)                   :: compute_rms_curve

  ! Working
  integer                            :: i, idim
  real(kind=realType)                :: val(ndim), D(ndim)

  compute_rms_curve = 0.0
  do i=1, n
     call eval_curve(s(i), t, k, coef, nctl, ndim, 1, val)
     D = val-X(:, i)
     do idim=1, ndim
        compute_rms_curve = compute_rms_curve + D(idim)**2
     end do
  end do
  compute_rms_curve = sqrt(compute_rms_curve/n)
end function compute_rms_curve