SUBROUTINE eta_nonlinear_3D_standing(H,angle,Lx,Ly,xc,yc,omega,time,eta,etax,etay,etaxx,etayy,etat)
!
! By Allan P. Engsig-Karup.
USE Precision
USE Constants
IMPLICIT NONE
REAL(KIND=long), INTENT(IN)  :: H,Lx,Ly,xc,yc,omega,time
REAL(KIND=long), INTENT(OUT) :: eta,etax,etay
REAL(KIND=long), INTENT(OUT) :: etaxx,etayy,etat
! REAL(KIND=long), OPTIONAL :: y,etay,etayy !Boundary fitted ?
! Local variables
REAL(KIND=long) :: kx, ky, half_H, k_omegat, sink_omegat
!
REAL(KIND=long) :: HH,k,w,angle,tmp,x,y,etaxy
!
x = xc*COS(-angle)-yc*SIN(-angle)
y = xc*SIN(-angle)+yc*COS(-angle)
!
kx    = two*pi/Lx/two; ky = zero*two*pi/Ly
k = SQRT(kx**2+ky**2)
w = omega
HH = H
!
!for now we use the linear solution...
IF (time==0.d0) THEN
!IF (0.d0==0.d0) THEN
eta = half*(0.8867_long*10.0**(-1)*COS(pi*X)+&
            SQRT(SQRT(two))*0.5243*10.0**(-2)*COS(two*pi*X)+&
            SQRT(SQRT(three))*0.4978*10.0**(-3)*COS(three*pi*X)+&
            SQRT(SQRT(four))*0.6542*10.0**(-4)*COS(four*pi*X)+&
            SQRT(SQRT(five))*0.1007*10.0**(-4)*COS(five*pi*X)+&
            SQRT(SQRT(six))*0.1653*10.0**(-5)*COS(six*pi*X)+&
            SQRT(SQRT(seven))*0.2753*10.0**(-6)*COS(seven*pi*X)+&
            SQRT(SQRT(eight))*0.4522*10.0**(-7)*COS(eight*pi*X))*COS(w*time)
etax = -half*(0.8867_long*10.0**(-1)*pi*SIN(pi*X)+&
            SQRT(SQRT(two))*0.5243*10.0**(-2)*(two*pi)*SIN(two*pi*X)+&
            SQRT(SQRT(three))*0.4978*10.0**(-3)*(three*pi)*SIN(three*pi*X)+&
            SQRT(SQRT(four))*0.6542*10.0**(-4)*(four*pi)*SIN(four*pi*X)+&
            SQRT(SQRT(five))*0.1007*10.0**(-4)*(five*pi)*SIN(five*pi*X)+&
            SQRT(SQRT(six))*0.1653*10.0**(-5)*(six*pi)*SIN(six*pi*X)+&
            SQRT(SQRT(seven))*0.2753*10.0**(-6)*(seven*pi)*SIN(seven*pi*X)+&
            SQRT(SQRT(eight))*0.4522*10.0**(-7)*(eight*pi)*SIN(eight*pi*X))*COS(w*time)
etaxx = -half*(0.8867_long*10.0**(-1)*pi**2*COS(pi*X)+&
            SQRT(SQRT(two))*0.5243*10.0**(-2)*(two*pi)**2*COS(two*pi*X)+&
            SQRT(SQRT(three))*0.4978*10.0**(-3)*(three*pi)**2*COS(three*pi*X)+&
            SQRT(SQRT(four))*0.6542*10.0**(-4)*(four*pi)**2*COS(four*pi*X)+&
            SQRT(SQRT(five))*0.1007*10.0**(-4)*(five*pi)**2*COS(five*pi*X)+&
            SQRT(SQRT(six))*0.1653*10.0**(-5)*(six*pi)**2*COS(six*pi*X)+&
            SQRT(SQRT(seven))*0.2753*10.0**(-6)*(seven*pi)**2*COS(seven*pi*X)+&
            SQRT(SQRT(eight))*0.4522*10.0**(-7)*(eight*pi)**2*COS(eight*pi*X))*COS(w*time)
etay  = zero
etayy = zero
etaxy = zero
etat  = -half*w*(0.8867_long*10.0**(-1)*COS(pi*X)+&
            SQRT(SQRT(two))*0.5243*10.0**(-2)*COS(two*pi*X)+&
            SQRT(SQRT(three))*0.4978*10.0**(-3)*COS(three*pi*X)+&
            SQRT(SQRT(four))*0.6542*10.0**(-4)*COS(four*pi*X)+&
            SQRT(SQRT(five))*0.1007*10.0**(-4)*COS(five*pi*X)+&
            SQRT(SQRT(six))*0.1653*10.0**(-5)*COS(six*pi*X)+&
            SQRT(SQRT(seven))*0.2753*10.0**(-6)*COS(seven*pi*X)+&
            SQRT(SQRT(eight))*0.4522*10.0**(-7)*COS(eight*pi*X))*SIN(w*time)
ELSE
  eta   =  half*0.08934064760240*COS(w*time)*COS(pi*X)
  etax  = -pi*half*0.08934064760240*COS(w*time)*SIN(pi*X)
  etaxx = -pi**2*half*0.08934064760240*COS(w*time)*COS(pi*X)
  etat  = -half*w*0.08934064760240*SIN(w*time)*COS(pi*X)
  etay  = zero
  etayy = zero
  etaxy = zero
ENDIF
!
tmp = etax
etax = tmp*COS(angle)-etay*SIN(angle)
etay = tmp*SIN(angle)+etay*COS(angle)
!
tmp = etaxx
etaxx = tmp*COS(angle)**2+etayy*SIN(angle)**2-2.0_long*COS(angle)*SIN(angle)*etaxy
etayy = tmp*SIN(angle)**2+etayy*COS(angle)**2+2.0_long*COS(angle)*SIN(angle)*etaxy
!xc = x*COS(angle)-y*SIN(angle)
!yc = x*SIN(angle)+y*COS(angle)

END SUBROUTINE eta_nonlinear_3D_standing


      SUBROUTINE lfit(x,y,sig,ndat,a,ia,ma,covar,npc,chisq,funcs)
      INTEGER ma,ia(ma),npc,ndat,MMAX
      REAL chisq,a(ma),covar(npc,npc),sig(ndat),x(ndat),y(ndat)
      EXTERNAL funcs
      PARAMETER (MMAX=50)
CU    USES covsrt,gaussj
      INTEGER i,j,k,l,m,mfit
      REAL sig2i,sum,wt,ym,afunc(MMAX),beta(MMAX)
      mfit=0
      do 11 j=1,ma
        if(ia(j).ne.0) mfit=mfit+1
11    continue
      if(mfit.eq.0) pause 'lfit: no parameters to be fitted'
      do 13 j=1,mfit
        do 12 k=1,mfit
          covar(j,k)=0.
12      continue
        beta(j)=0.
13    continue
      do 17 i=1,ndat
        call funcs(x(i),afunc,ma)
        ym=y(i)
        if(mfit.lt.ma) then
          do 14 j=1,ma
            if(ia(j).eq.0) ym=ym-a(j)*afunc(j)
14        continue
        endif
        sig2i=1./sig(i)**2
        j=0
        do 16 l=1,ma
          if (ia(l).ne.0) then
            j=j+1
            wt=afunc(l)*sig2i
            k=0
            do 15 m=1,l
              if (ia(m).ne.0) then
                k=k+1
                covar(j,k)=covar(j,k)+wt*afunc(m)
              endif
15          continue
            beta(j)=beta(j)+ym*wt
          endif
16      continue
17    continue
      do 19 j=2,mfit
        do 18 k=1,j-1
          covar(k,j)=covar(j,k)
18      continue
19    continue
      call gaussj(covar,mfit,npc,beta,1,1)
      j=0
      do 21 l=1,ma
        if(ia(l).ne.0) then
          j=j+1
          a(l)=beta(j)
        endif
21    continue
      chisq=0.
      do 23 i=1,ndat
        call funcs(x(i),afunc,ma)
        sum=0.
        do 22 j=1,ma
          sum=sum+a(j)*afunc(j)
22      continue
        chisq=chisq+((y(i)-sum)/sig(i))**2
23    continue
      call covsrt(covar,npc,ma,ia,mfit)
      return
      END

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


!*==zptrfs.f90  processed by SPAG 7.51RB at 20:08 on  3 Mar 2022
!> \brief \b ZPTRFS
!
!  =========== DOCUMENTATION ===========
!
! Online html documentation available at
!            http://www.netlib.org/lapack/explore-html/
!
!> \htmlonly
!> Download ZPTRFS + dependencies
!>
!> [TGZ]
!>
!> [ZIP]
!>
!> [TXT]
!> \endhtmlonly
!
!  Definition:
!  ===========
!
!       SUBROUTINE ZPTRFS( UPLO, N, NRHS, D, E, DF, EF, B, LDB, X, LDX,
!                          FERR, BERR, WORK, RWORK, INFO )
!
!       .. Scalar Arguments ..
!       CHARACTER          UPLO
!       INTEGER            INFO, LDB, LDX, N, NRHS
!       ..
!       .. Array Arguments ..
!       DOUBLE PRECISION   BERR( * ), D( * ), DF( * ), FERR( * ),
!      $                   RWORK( * )
!       COMPLEX*16         B( LDB, * ), E( * ), EF( * ), WORK( * ),
!      $                   X( LDX, * )
!       ..
!
!
!> \par Purpose:
!  =============
!>
!> \verbatim
!>
!> ZPTRFS improves the computed solution to a system of linear
!> equations when the coefficient matrix is Hermitian positive definite
!> and tridiagonal, and provides error bounds and backward error
!> estimates for the solution.
!> \endverbatim
!
!  Arguments:
!  ==========
!
!> \param[in] UPLO
!> \verbatim
!>          UPLO is CHARACTER*1
!>          Specifies whether the superdiagonal or the subdiagonal of the
!>          tridiagonal matrix A is stored and the form of the
!>          factorization:
!>          = 'U':  E is the superdiagonal of A, and A = U**H*D*U;
!>          = 'L':  E is the subdiagonal of A, and A = L*D*L**H.
!>          (The two forms are equivalent if A is real.)
!> \endverbatim
!>
!> \param[in] N
!> \verbatim
!>          N is INTEGER
!>          The order of the matrix A.  N >= 0.
!> \endverbatim
!>
!> \param[in] NRHS
!> \verbatim
!>          NRHS is INTEGER
!>          The number of right hand sides, i.e., the number of columns
!>          of the matrix B.  NRHS >= 0.
!> \endverbatim
!>
!> \param[in] D
!> \verbatim
!>          D is DOUBLE PRECISION array, dimension (N)
!>          The n real diagonal elements of the tridiagonal matrix A.
!> \endverbatim
!>
!> \param[in] E
!> \verbatim
!>          E is COMPLEX*16 array, dimension (N-1)
!>          The (n-1) off-diagonal elements of the tridiagonal matrix A
!>          (see UPLO).
!> \endverbatim
!>
!> \param[in] DF
!> \verbatim
!>          DF is DOUBLE PRECISION array, dimension (N)
!>          The n diagonal elements of the diagonal matrix D from
!>          the factorization computed by ZPTTRF.
!> \endverbatim
!>
!> \param[in] EF
!> \verbatim
!>          EF is COMPLEX*16 array, dimension (N-1)
!>          The (n-1) off-diagonal elements of the unit bidiagonal
!>          factor U or L from the factorization computed by ZPTTRF
!>          (see UPLO).
!> \endverbatim
!>
!> \param[in] B
!> \verbatim
!>          B is COMPLEX*16 array, dimension (LDB,NRHS)
!>          The right hand side matrix B.
!> \endverbatim
!>
!> \param[in] LDB
!> \verbatim
!>          LDB is INTEGER
!>          The leading dimension of the array B.  LDB >= max(1,N).
!> \endverbatim
!>
!> \param[in,out] X
!> \verbatim
!>          X is COMPLEX*16 array, dimension (LDX,NRHS)
!>          On entry, the solution matrix X, as computed by ZPTTRS.
!>          On exit, the improved solution matrix X.
!> \endverbatim
!>
!> \param[in] LDX
!> \verbatim
!>          LDX is INTEGER
!>          The leading dimension of the array X.  LDX >= max(1,N).
!> \endverbatim
!>
!> \param[out] FERR
!> \verbatim
!>          FERR is DOUBLE PRECISION array, dimension (NRHS)
!>          The forward error bound for each solution vector
!>          X(j) (the j-th column of the solution matrix X).
!>          If XTRUE is the true solution corresponding to X(j), FERR(j)
!>          is an estimated upper bound for the magnitude of the largest
!>          element in (X(j) - XTRUE) divided by the magnitude of the
!>          largest element in X(j).
!> \endverbatim
!>
!> \param[out] BERR
!> \verbatim
!>          BERR is DOUBLE PRECISION array, dimension (NRHS)
!>          The componentwise relative backward error of each solution
!>          vector X(j) (i.e., the smallest relative change in
!>          any element of A or B that makes X(j) an exact solution).
!> \endverbatim
!>
!> \param[out] WORK
!> \verbatim
!>          WORK is COMPLEX*16 array, dimension (N)
!> \endverbatim
!>
!> \param[out] RWORK
!> \verbatim
!>          RWORK is DOUBLE PRECISION array, dimension (N)
!> \endverbatim
!>
!> \param[out] INFO
!> \verbatim
!>          INFO is INTEGER
!>          = 0:  successful exit
!>          < 0:  if INFO = -i, the i-th argument had an illegal value
!> \endverbatim
!
!> \par Internal Parameters:
!  =========================
!>
!> \verbatim
!>  ITMAX is the maximum number of steps of iterative refinement.
!> \endverbatim
!
!  Authors:
!  ========
!
!> \author Univ. of Tennessee
!> \author Univ. of California Berkeley
!> \author Univ. of Colorado Denver
!> \author NAG Ltd.
!
!> \date December 2016
!
!> \ingroup complex16PTcomputational
!
!  =====================================================================
      SUBROUTINE ZPTRFS(Uplo,N,Nrhs,D,E,Df,Ef,B,Ldb,X,Ldx,Ferr,Berr,    &
     &                  Work,Rwork,Info)
      IMPLICIT NONE
!*--ZPTRFS187
!
!  -- LAPACK computational 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 ..
      CHARACTER Uplo
      INTEGER Info , Ldb , Ldx , N , Nrhs
!     ..
!     .. Array Arguments ..
      DOUBLE PRECISION Berr(*) , D(*) , Df(*) , Ferr(*) , Rwork(*)
      COMPLEX*16 B(Ldb,*) , E(*) , Ef(*) , Work(*) , X(Ldx,*)
!     ..
!
!  =====================================================================
!
!     .. Parameters ..
      INTEGER ITMAX
      PARAMETER (ITMAX=5)
      DOUBLE PRECISION ZERO
      PARAMETER (ZERO=0.0D+0)
      DOUBLE PRECISION ONE
      PARAMETER (ONE=1.0D+0)
      DOUBLE PRECISION TWO
      PARAMETER (TWO=2.0D+0)
      DOUBLE PRECISION THREE
      PARAMETER (THREE=3.0D+0)
!     ..
!     .. Local Scalars ..
      LOGICAL upper
      INTEGER count , i , ix , j , nz
      DOUBLE PRECISION eps , lstres , s , safe1 , safe2 , safmin
      COMPLEX*16 bi , cx , dx , ex , zdum
!     ..
!     .. External Functions ..
      LOGICAL LSAME
      INTEGER IDAMAX
      DOUBLE PRECISION DLAMCH
      EXTERNAL LSAME , IDAMAX , DLAMCH
!     ..
!     .. External Subroutines ..
      EXTERNAL XERBLA , ZAXPY , ZPTTRS
!     ..
!     .. Intrinsic Functions ..
      INTRINSIC ABS , DBLE , DCMPLX , DCONJG , DIMAG , MAX
!     ..
!     .. Statement Functions ..
      DOUBLE PRECISION CABS1
!     ..
!     .. Statement Function definitions ..
      CABS1(zdum) = ABS(DBLE(zdum)) + ABS(DIMAG(zdum))
!     ..
!     .. Executable Statements ..
!
!     Test the input parameters.
!
      Info = 0
      upper = LSAME(Uplo,'U')
      IF ( .NOT.upper .AND. .NOT.LSAME(Uplo,'L') ) THEN
         Info = -1
      ELSEIF ( N<0 ) THEN
         Info = -2
      ELSEIF ( Nrhs<0 ) THEN
         Info = -3
      ELSEIF ( Ldb<MAX(1,N) ) THEN
         Info = -9
      ELSEIF ( Ldx<MAX(1,N) ) THEN
         Info = -11
      ENDIF
      IF ( Info/=0 ) THEN
         CALL XERBLA('ZPTRFS',-Info)
         RETURN
      ENDIF
!
!     Quick return if possible
!
      IF ( N==0 .OR. Nrhs==0 ) THEN
         DO j = 1 , Nrhs
            Ferr(j) = ZERO
            Berr(j) = ZERO
         ENDDO
         RETURN
      ENDIF
!
!     NZ = maximum number of nonzero elements in each row of A, plus 1
!
      nz = 4
      eps = DLAMCH('Epsilon')
      safmin = DLAMCH('Safe minimum')
      safe1 = nz*safmin
      safe2 = safe1/eps
!
!     Do for each right hand side
!
      DO j = 1 , Nrhs
!
         count = 1
         lstres = THREE
         DO
!
!        Loop until stopping criterion is satisfied.
!
!        Compute residual R = B - A * X.  Also compute
!        abs(A)*abs(x) + abs(b) for use in the backward error bound.
!
            IF ( upper ) THEN
               IF ( N==1 ) THEN
                  bi = B(1,j)
                  dx = D(1)*X(1,j)
                  Work(1) = bi - dx
                  Rwork(1) = CABS1(bi) + CABS1(dx)
               ELSE
                  bi = B(1,j)
                  dx = D(1)*X(1,j)
                  ex = E(1)*X(2,j)
                  Work(1) = bi - dx - ex
                  Rwork(1) = CABS1(bi) + CABS1(dx) + CABS1(E(1))        &
     &                       *CABS1(X(2,j))
                  DO i = 2 , N - 1
                     bi = B(i,j)
                     cx = DCONJG(E(i-1))*X(i-1,j)
                     dx = D(i)*X(i,j)
                     ex = E(i)*X(i+1,j)
                     Work(i) = bi - cx - dx - ex
                     Rwork(i) = CABS1(bi) + CABS1(E(i-1))               &
     &                          *CABS1(X(i-1,j)) + CABS1(dx)            &
     &                          + CABS1(E(i))*CABS1(X(i+1,j))
                  ENDDO
                  bi = B(N,j)
                  cx = DCONJG(E(N-1))*X(N-1,j)
                  dx = D(N)*X(N,j)
                  Work(N) = bi - cx - dx
                  Rwork(N) = CABS1(bi) + CABS1(E(N-1))*CABS1(X(N-1,j))  &
     &                       + CABS1(dx)
               ENDIF
            ELSEIF ( N==1 ) THEN
               bi = B(1,j)
               dx = D(1)*X(1,j)
               Work(1) = bi - dx
               Rwork(1) = CABS1(bi) + CABS1(dx)
            ELSE
               bi = B(1,j)
               dx = D(1)*X(1,j)
               ex = DCONJG(E(1))*X(2,j)
               Work(1) = bi - dx - ex
               Rwork(1) = CABS1(bi) + CABS1(dx) + CABS1(E(1))           &
     &                    *CABS1(X(2,j))
               DO i = 2 , N - 1
                  bi = B(i,j)
                  cx = E(i-1)*X(i-1,j)
                  dx = D(i)*X(i,j)
                  ex = DCONJG(E(i))*X(i+1,j)
                  Work(i) = bi - cx - dx - ex
                  Rwork(i) = CABS1(bi) + CABS1(E(i-1))*CABS1(X(i-1,j))  &
     &                       + CABS1(dx) + CABS1(E(i))*CABS1(X(i+1,j))
               ENDDO
               bi = B(N,j)
               cx = E(N-1)*X(N-1,j)
               dx = D(N)*X(N,j)
               Work(N) = bi - cx - dx
               Rwork(N) = CABS1(bi) + CABS1(E(N-1))*CABS1(X(N-1,j))     &
     &                    + CABS1(dx)
            ENDIF
!
!        Compute componentwise relative backward error from formula
!
!        max(i) ( abs(R(i)) / ( abs(A)*abs(X) + abs(B) )(i) )
!
!        where abs(Z) is the componentwise absolute value of the matrix
!        or vector Z.  If the i-th component of the denominator is less
!        than SAFE2, then SAFE1 is added to the i-th components of the
!        numerator and denominator before dividing.
!
            s = ZERO
            DO i = 1 , N
               IF ( Rwork(i)>safe2 ) THEN
                  s = MAX(s,CABS1(Work(i))/Rwork(i))
               ELSE
                  s = MAX(s,(CABS1(Work(i))+safe1)/(Rwork(i)+safe1))
               ENDIF
            ENDDO
            Berr(j) = s
!
!        Test stopping criterion. Continue iterating if
!           1) The residual BERR(J) is larger than machine epsilon, and
!           2) BERR(J) decreased by at least a factor of 2 during the
!              last iteration, and
!           3) At most ITMAX iterations tried.
!
            IF ( Berr(j)>eps .AND. TWO*Berr(j)<=lstres .AND.            &
     &           count<=ITMAX ) THEN
!
!           Update solution and try again.
!
               CALL ZPTTRS(Uplo,N,1,Df,Ef,Work,N,Info)
               CALL ZAXPY(N,DCMPLX(ONE),Work,1,X(1,j),1)
               lstres = Berr(j)
               count = count + 1
               CYCLE
            ENDIF
!
!        Bound error from formula
!
!        norm(X - XTRUE) / norm(X) .le. FERR =
!        norm( abs(inv(A))*
!           ( abs(R) + NZ*EPS*( abs(A)*abs(X)+abs(B) ))) / norm(X)
!
!        where
!          norm(Z) is the magnitude of the largest component of Z
!          inv(A) is the inverse of A
!          abs(Z) is the componentwise absolute value of the matrix or
!             vector Z
!          NZ is the maximum number of nonzeros in any row of A, plus 1
!          EPS is machine epsilon
!
!        The i-th component of abs(R)+NZ*EPS*(abs(A)*abs(X)+abs(B))
!        is incremented by SAFE1 if the i-th component of
!        abs(A)*abs(X) + abs(B) is less than SAFE2.
!
            DO i = 1 , N
               IF ( Rwork(i)>safe2 ) THEN
                  Rwork(i) = CABS1(Work(i)) + nz*eps*Rwork(i)
               ELSE
                  Rwork(i) = CABS1(Work(i)) + nz*eps*Rwork(i) + safe1
               ENDIF
            ENDDO
            ix = IDAMAX(N,Rwork,1)
            Ferr(j) = Rwork(ix)
!
!        Estimate the norm of inv(A).
!
!        Solve M(A) * x = e, where M(A) = (m(i,j)) is given by
!
!           m(i,j) =  abs(A(i,j)), i = j,
!           m(i,j) = -abs(A(i,j)), i .ne. j,
!
!        and e = [ 1, 1, ..., 1 ]**T.  Note M(A) = M(L)*D*M(L)**H.
!
!        Solve M(L) * x = e.
!
            Rwork(1) = ONE
            DO i = 2 , N
               Rwork(i) = ONE + Rwork(i-1)*ABS(Ef(i-1))
            ENDDO
!
!        Solve D * M(L)**H * x = b.
!
            Rwork(N) = Rwork(N)/Df(N)
            DO i = N - 1 , 1 , -1
               Rwork(i) = Rwork(i)/Df(i) + Rwork(i+1)*ABS(Ef(i))
            ENDDO
!
!        Compute norm(inv(A)) = max(x(i)), 1<=i<=n.
!
            ix = IDAMAX(N,Rwork,1)
            Ferr(j) = Ferr(j)*ABS(Rwork(ix))
!
!        Normalize error.
!
            lstres = ZERO
            DO i = 1 , N
               lstres = MAX(lstres,ABS(X(i,j)))
            ENDDO
            IF ( lstres/=ZERO ) Ferr(j) = Ferr(j)/lstres
            EXIT
         ENDDO
!
      ENDDO
!
!
!     End of ZPTRFS
!
      END SUBROUTINE ZPTRFS


#include "eiscor.h"
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!
! z_2x2array_eig
!
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!
! This routine computes the generalized Schur decomposition of a
! 2x2 matrix pencil (A,B). On exit the pencil (A,B) will be in upper-
! triangular form.
!
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!
! INPUT VARIABLES:
!
!  FLAG           LOGICAL
!                    .TRUE. makes no assumptions on B (General)
!                    .FALSE. assumes B = I
!
!  A, B           COMPLEX(8) array of dimension (2,2)
!                   The 2x2 matrix pencil. Upper-triangular on exit.
!                   If FLAG = .FALSE., B is not used.
!
! OUTPUT VARIABLES:
!
!  Q, Z           COMPLEX(8) array of dimension (2,2)
!                   On exit unitary transformations such that Q*(A,B)Z is
!                   upper-triangular. If FLAG = .FALSE., Z is unused.
!
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
subroutine z_2x2array_eig(FLAG,A,B,Q,Z)

  implicit none

  ! input variables
  logical, intent(in) :: FLAG
  complex(8), intent(inout) :: A(2,2), B(2,2), Q(2,2), Z(2,2)

  ! compute variables
  real(8) :: nrm, cr, ci, s
  complex(8) :: temp(2,2), p0, p1, p2, lambda
  complex(8) :: trace, detm, disc

  ! B not the identity
  if (FLAG) then

    ! make B upper-triangular
    ! create eliminator
    call z_rot3_vec4gen(dble(B(1,1)),aimag(B(1,1)),dble(B(2,1)) &
    ,aimag(B(2,1)),cr,ci,s,nrm)

    ! store in Q
    Q(1,1) = cmplx(cr,ci,kind=8)
    Q(2,1) = cmplx(s,0d0,kind=8)
    Q(2,2) = conjg(Q(1,1))
    Q(1,2) = -Q(2,1)

    ! update B
    B = matmul(conjg(transpose(Q)),B)
    B(2,1) = cmplx(0d0,0d0,kind=8)

    ! update A
    A = matmul(conjg(transpose(Q)),A)

    ! set Z
    Z(1,1) = cmplx(1d0,0d0,kind=8)
    Z(2,1) = cmplx(0d0,0d0,kind=8)
    Z(2,2) = cmplx(1d0,0d0,kind=8)
    Z(1,2) = cmplx(0d0,0d0,kind=8)

    ! check for infinite eigenvalues
    if (abs(B(1,1)).EQ.0d0) then

      ! create eliminator
      call z_rot3_vec4gen(dble(A(1,1)),aimag(A(1,1)),dble(A(2,1)) &
      ,aimag(A(2,1)),cr,ci,s,nrm)

      ! store in temp
      temp(1,1) = cmplx(cr,ci,kind=8)
      temp(2,1) = cmplx(s,0d0,kind=8)
      temp(2,2) = conjg(temp(1,1))
      temp(1,2) = -temp(2,1)

      ! update A
      A = matmul(conjg(transpose(temp)),A)
      A(2,1) = cmplx(0d0,0d0,kind=8)

      ! update B
      B = matmul(conjg(transpose(Q)),B)

      ! update Q
      Q = matmul(Q,temp)

      ! return
      return

    ! check for infinite eigenvalues
    else if (abs(B(2,2)).EQ.0d0) then

      ! create eliminator
      call z_rot3_vec4gen(dble(A(2,2)),aimag(A(2,2)),dble(A(2,1)) &
      ,aimag(A(2,1)),cr,ci,s,nrm)

      ! store in temp
      temp(1,1) = cmplx(cr,-ci,kind=8)
      temp(2,1) = cmplx(s,0d0,kind=8)
      temp(2,2) = conjg(temp(1,1))
      temp(1,2) = -temp(2,1)

      ! update A
      A = matmul(A,temp)
      A(2,1) = cmplx(0d0,0d0,kind=8)

      ! update B
      B = matmul(B,temp)

      ! update Z
      Z = matmul(Z,temp)

      ! return
      return

    ! compute eigenvalues otherwise
    else

      ! compute polynomial coefficients
      p2 = B(1,1)*B(2,2)
      p1 = A(2,1)*B(1,2) - (A(1,1)*B(2,2)+A(2,2)*B(1,1))
      p0 = A(1,1)*A(2,2) - A(2,1)*A(1,2)

      ! compute intermediate values
      disc = sqrt(p1**2 - 4d0*p2*p0)

      ! compute most accurate lambda
      if(abs(-p1+disc) > abs(-p1-disc))then
        lambda = (-p1+disc)/2d0/p2
      else
        lambda = (-p1-disc)/2d0/p2
      end if

      ! compute right eigenvector
      temp = A - lambda*B

      ! create eliminator
      call z_rot3_vec4gen(dble(temp(1,1)),-aimag(temp(1,1)) &
      ,dble(temp(1,2)),-aimag(temp(1,2)),cr,ci,s,nrm)

      ! store in temp
      temp(1,1) = cmplx(-s,0d0,kind=8)
      temp(2,1) = cmplx(cr,-ci,kind=8)
      temp(1,2) = cmplx(cr,ci,kind=8)
      temp(2,2) = cmplx(s,0d0,kind=8)

      ! update A
      A = matmul(A,temp)

      ! update B
      B = matmul(B,temp)

      ! update Z
      Z = matmul(Z,temp)

      ! return to upper-triangular form
      ! create eliminator
      call z_rot3_vec4gen(dble(A(1,1)),aimag(A(1,1)),dble(A(2,1)) &
      ,aimag(A(2,1)),cr,ci,s,nrm)

      ! store in temp
      temp(1,1) = cmplx(cr,ci,kind=8)
      temp(2,1) = cmplx(s,0d0,kind=8)
      temp(1,2) = cmplx(-s,0d0,kind=8)
      temp(2,2) = cmplx(cr,-ci,kind=8)

      ! update A
      A = matmul(conjg(transpose(temp)),A)
      A(2,1) = cmplx(0d0,0d0,kind=8)

      ! update B
      B = matmul(conjg(transpose(temp)),B)
      B(2,1) = cmplx(0d0,0d0,kind=8)

      ! update Q
      Q = matmul(Q,temp)

    end if

  ! if B = I
  else

    ! compute intermediate values
    trace = A(1,1) + A(2,2)
    detm = A(1,1)*A(2,2) - A(2,1)*A(1,2)
    disc = sqrt((A(1,1)-A(2,2))**2 + 4d0*A(1,2)*A(2,1))

    ! compute most accurate eigenvalue
    if(abs(trace+disc) > abs(trace-disc))then
      lambda = (trace+disc)/2d0
    else
      lambda = (trace-disc)/2d0
    end if

    ! compute Q
    lambda = lambda-A(1,1)
    call z_rot3_vec4gen(dble(A(1,2)),aimag(A(1,2)),dble(lambda) &
    ,aimag(lambda),cr,ci,s,nrm)
    Q(1,1) = cmplx(cr,ci,kind=8)
    Q(2,1) = cmplx(s,0d0,kind=8)
    Q(2,2) = conjg(Q(1,1))
    Q(1,2) = -Q(2,1)

    ! update A
    A = matmul(transpose(conjg(Q)),matmul(A,Q))
    A(2,1) = cmplx(0d0,0d0,kind=8)

  end if

end subroutine z_2x2array_eig


    Program zhegvx_example

!     ZHEGVX Example Program Text

!     Copyright (c) 2018, Numerical Algorithms Group (NAG Ltd.)
!     For licence see
!       https://github.com/numericalalgorithmsgroup/LAPACK_Examples/blob/master/LICENCE.md

!     .. Use Statements ..
      Use lapack_example_aux, Only: nagf_file_print_matrix_complex_gen
      Use lapack_interfaces, Only: zhegvx
      Use lapack_precision, Only: dp
!     .. Implicit None Statement ..
      Implicit None
!     .. Parameters ..
      Real (Kind=dp), Parameter :: zero = 0.0E+0_dp
      Integer, Parameter :: nb = 64, nin = 5, nout = 6
!     .. Local Scalars ..
      Complex (Kind=dp) :: scal
      Real (Kind=dp) :: abstol, vl, vu
      Integer :: i, ifail, il, info, iu, k, lda, ldb, ldz, lwork, m, n
!     .. Local Arrays ..
      Complex (Kind=dp), Allocatable :: a(:, :), b(:, :), work(:), z(:, :)
      Complex (Kind=dp) :: dummy(1)
      Real (Kind=dp), Allocatable :: rwork(:), w(:)
      Integer, Allocatable :: iwork(:), jfail(:)
!     .. Intrinsic Procedures ..
      Intrinsic :: abs, conjg, max, maxloc, nint, real
!     .. Executable Statements ..
      Write (nout, *) 'ZHEGVX Example Program Results'
      Write (nout, *)
!     Skip heading in data file
      Read (nin, *)
      Read (nin, *) n
      lda = n
      ldb = n
      ldz = n
      m = n
      Allocate (a(lda,n), b(ldb,n), z(ldz,m), rwork(7*n), w(n), iwork(5*n), &
        jfail(n))

!     Read the lower and upper bounds of the interval to be searched.
      Read (nin, *) vl, vu

!     Use routine workspace query to get optimal workspace.
      lwork = -1
      Call zhegvx(1, 'Vectors', 'Values in range', 'Upper', n, a, lda, b, ldb, &
        vl, vu, il, iu, abstol, m, w, z, ldz, dummy, lwork, rwork, iwork, &
        jfail, info)

!     Make sure that there is enough workspace for block size nb.
      lwork = max((nb+1)*n, nint(real(dummy(1))))
      Allocate (work(lwork))

!     Read the upper triangular parts of the matrices A and B

      Read (nin, *)(a(i,i:n), i=1, n)
      Read (nin, *)(b(i,i:n), i=1, n)

!     Set the absolute error tolerance for eigenvalues.  With abstol
!     set to zero, the default value is used instead

      abstol = zero

!     Solve the generalized Hermitian eigenvalue problem
!     A*x = lambda*B*x (itype = 1)

      Call zhegvx(1, 'Vectors', 'Values in range', 'Upper', n, a, lda, b, ldb, &
        vl, vu, il, iu, abstol, m, w, z, ldz, work, lwork, rwork, iwork, &
        jfail, info)

      If (info>=0 .And. info<=n) Then

!       Print solution

        Write (nout, 100) 'Number of eigenvalues found =', m
        Write (nout, *)
        Write (nout, *) 'Eigenvalues'
        Write (nout, 110) w(1:m)
        Flush (nout)

!       Normalize the eigenvectors, largest element real
        Do i = 1, m
          rwork(1:n) = abs(z(1:n,i))
          k = maxloc(rwork(1:n), 1)
          scal = conjg(z(k,i))/abs(z(k,i))
          z(1:n, i) = z(1:n, i)*scal
        End Do

!       ifail: behaviour on error exit
!              =0 for hard exit, =1 for quiet-soft, =-1 for noisy-soft
        ifail = 0
        Call nagf_file_print_matrix_complex_gen('General', ' ', n, m, z, ldz, &
          'Selected eigenvectors', ifail)

        If (info>0) Then
          Write (nout, 100) 'INFO eigenvectors failed to converge, INFO =', &
            info
          Write (nout, *) 'Indices of eigenvectors that did not converge'
          Write (nout, 120) jfail(1:m)
        End If
      Else If (info>n .And. info<=2*n) Then
        i = info - n
        Write (nout, 130) 'The leading minor of order ', i, &
          ' of B is not positive definite'
      Else
        Write (nout, 100) 'Failure in ZHEGVX. INFO =', info
      End If

100   Format (1X, A, I5)
110   Format (3X, (8F8.4))
120   Format (3X, (8I8))
130   Format (1X, A, I4, A)
    End Program


! This module definbes the global_variables which are used at different places int he other modules
module global_variables
    implicit none

    ! CONSTANTS
    double precision, parameter :: kb = 1.38064852e-23, EE = 1.6021766208e-19, PI = 3.14159265, KE = 8.99e9, NA = 6.02214086e23
    double precision, parameter :: P = 101325.0, Mg = 29.2e-3
    ! KB = boltzmann Constant (SI)
    ! EE = elementary charge on an electron
    ! KE = 1/(4.0*PI*eps0)
    ! NA = Avagadro's Constant (mol-1)
    ! P = Pressure (N/m2)
    ! Mg = Molecular Weight of gas (Kg/mol)

    ! MOLECULAR WEIGHT AND DENSITY
    double precision :: M , rho , T , gam , Ps, A, B , r_rate
    integer :: nlines
    double precision, dimension(:), allocatable :: vz_pos, z_pos, time, Temperature, vapor_conc
    !double precision :: time(5), Temperature(5), vapor_conc(5)
    double precision :: num_zero_0 , dpg_zero_0 , sigma_g_zero_0 , vapor_zero_0
    ! M = Molecular Weight (Kg/mol)
    ! rho = Density (Kg/m3)
	! T = Temperature (K)
    ! gam = surface tension (N/m)
	! Ps = saturation pressure (N/m2)
	! r_rate = reaction rate

    ! INITIAL
    double precision :: num_zero , dpg_zero, sigma_g_zero, S_zero
	! num_zero = Initial number concentration
    ! dpg_zero = Initial geometric mean diameter
	! sigma_g_zero = Initial geometric standard deviation
	! S_zero = Initial saturation ratio

    ! OTHERS
    integer :: flag_COAG , flag_COND , flag_NUCL , flag_REAC ! defines flags for different inputs

    ! SOLVER VARIABLES
    double precision ::  t0 = 0.0 , dt = 0.01
	! S = saturation ratio
    !integer :: istate , itask = 1, neq

    ! Non-Dimensional Parameters
    !double precision :: K_coag = 8.0*KB*1200.0/(3.0*1.81e-5)
    !double precision :: N_infi_0, tau,
    !double precision :: tau_mono

    ! For beta calculation storage
    !double precision, dimension(:,:), allocatable :: beta_all
    !double precision :: N_infi_0, K_coag, tau, N_track_min
    ! beta_all is the matrix which contains all the values of the beta
    ! beta_safe is the module
    ! beta is the function which finds the value of beta
    ! fill_beta is the subroutine which fills in all the values of beta

end module global_variables

!program test_global_variables
!    use global_variables
!    implicit none
!
!    print*, t0
!
!end program test_global_variables


      SUBROUTINE PLMPSI(L,M,PSI,P,DPDPS)

!PURPOSE: Compute P(l,m,psi)

      INCLUDE       '../INCLDS/COPYRIGT.FOR'

!CALLING ARGUMENTS:

      INTEGER*4     L,M                 !Order & index
      REAL*4        PSI                 !Angle (in deg)
      REAL*4        P                   !Value returned

!INCLUDE STATEMENTS:

!LOCAL VARIABLES:

      INTEGER*4     S
      REAL*4        APR,RS

!FUNCTION DEFINITIONS:

!DATA STATEMENTS:

!CODE:

      P = 0.0
      DPDPS = 0.0
      IF ( MOD(ABS(M),2).EQ.0 ) THEN
        DO S=0,L,2
          CALL APLMS(L,M,S,APR)
          RS = S
          P = P+APR*COSD(RS*PSI)
          DPDPS = DPDPS-RS*APR*SIND(RS*PSI)
        END DO
      ELSE
        DO S=2,L,2
          CALL APLMS(L,M,S,APR)
          RS = S
          P = P+APR*SIND(RS*PSI)
          DPDPS = DPDPS+RS*APR*COSD(RS*PSI)
        END DO
      END IF
      RETURN
      END


program colortest

use mod_EMsoft
use mod_kinds
use mod_global
use mod_colorspace
use mod_image
use mod_io
use ISO_C_BINDING
use mod_io
use, intrinsic :: iso_fortran_env

IMPLICIT NONE

type(colorspace_T)      :: clr
real(kind=dbl)          :: xyz(0:2), rgb(0:2), lab(0:2), luv(0:2), hsv(0:2), hsl(0:2), a, p
real(kind=dbl)          :: xyz2(0:2), rgb2(0:2), lab2(0:2), luv2(0:2), hsv2(0:2), hsl2(0:2)

character(fnlen)        :: progname = 'colortest.f90'
character(fnlen)        :: progdesc = 'Standalone test program to generate a stereographic color projection '

type(IO_T)                                  :: Message
type(EMsoft_T)                              :: EMsoft

character(fnlen)                            :: fname, TIFF_filename
real(kind=dbl),allocatable                  :: IPFmap(:,:,:)
integer(kind=irg)                           :: i, j, N, NN
real(kind=dbl)                              :: ix, iy, R, RR, X, Y, Z, phi, theta, mina, maxa, minp, maxp

! declare variables for use in object oriented image module
integer                                     :: iostat, io_int(2)
character(len=128)                          :: iomsg
logical                                     :: isInteger, OPC
type(image_t)                               :: im
integer(int8), allocatable                  :: TIFF_image(:,:)
integer                                     :: dim2(2), Pm
integer(c_int32_t)                          :: result

EMsoft = EMsoft_T( progname, progdesc )

! initialize the color space class
clr = colorspace_T( Nfold = 6 )

! write (*,*) '================'
write (*,*) 'Test of perceptually uniform color scheme code '

p = 0.05D0
do i = 0,11
    a = ( dble(i) * cPi / 6.D0 ) / (2.D0 * cPi)
    luv = clr%sphere2rgb(a, p, w0=.TRUE.)
    ! write (*,*)  a, p, luv
end do

N = 501
NN = (N-1)/2
R = dble(NN)
allocate( IPFmap(3, -NN:NN, -NN:NN) )

mina = 100.D0
maxa = -100.D0
minp = 100.D0
maxp = -100.D0

do i=-NN,NN
    ix = dble(i)/R
    do j=-NN,NN
      iy = dble(j)/R
      RR = ix*ix+iy*iy
      if (RR.le.1.D0) then ! convert to azimuth and polar angles on [0,1]
! coordinates on the sphere
        X = 2.D0*ix/(1.D0+RR)
        Y = 2.D0*iy/(1.D0+RR)
        Z = -(-1.D0+RR)/(1.D0+RR)
        a = acos(Z)/cPi
        p = atan2(Y,X)
        if (p.lt.0.D0) p = p + 2.D0*cPi
        p = p/(2.D0*cPi)
        if (a.lt.mina) mina = a
        if (a.gt.maxa) maxa = a
        if (p.lt.minp) minp = p
        if (p.gt.maxp) maxp = p
        rgb = clr%sphere2rgb(a, p, w0=.TRUE.)
        IPFmap(1:3,i,j) = rgb(0:2)*255.D0
        ! IPFmap(1:3,i,j) = (/  a, p, 0.D0 /)*255.D0
      end if
    end do
end do

write (*,*) ' range of azimuth/polar : ', mina, maxa, minp, maxp

! finally, store the IPFmap in a color TIFF file.
fname = trim(EMsoft%generateFilePath('EMdatapathname'))//'../EMsoftOObuild/PUC_NH.tiff'
TIFF_filename = trim(fname)

! allocate memory for a color image; each pixel has 3 bytes [RGB]
allocate(TIFF_image(3*N,N))
TIFF_image = reshape( IPFmap, (/ 3*N, N /) )

! set up the image_t structure
im = image_t(TIFF_image)
im%dims = (/ N, N /)
im%samplesPerPixel = 3
if(im%empty()) call Message%printMessage("EMdpmerge: failed to convert array to rgb image")

! create the file
call im%write(trim(TIFF_filename), iostat, iomsg) ! format automatically detected from extension
if(0.ne.iostat) then
  call Message%printMessage(" Failed to write image to file : "//iomsg)
else
  call Message%printMessage(' IPF map written to '//trim(TIFF_filename),"(/A)")
end if

end program colortest


      subroutine tridiag(a,b,c,r,u,n)

      integer n,NMAX
*     !i/o
      double precision a(n),b(n),c(n),r(n),u(n)
*     !i/o
      PARAMETER (NMAX = 5000)
      integer j
      double precision bet,gam(NMAX)

      if (b(1).eq.0) write(*,*) 'tridiag: rewrite equations'
      bet = b(1)
      u(1) = r(1)/bet

      do j = 2,n
         gam(j) = c(j-1)/bet
         bet = b(j) - a(j)*gam(j)
         if (bet.eq.0.) then
            write(*,*) 'tridiag failed'
            stop
         endif
         u(j) = (r(j)-a(j)*u(j-1))/bet
      enddo

      do j = n-1,1,-1
         u(j) = u(j) - gam(j+1)*u(j+1)
      enddo

      return
      end


subroutine setup_mg_geometry
 use global_variables
 use multigrid_levels
 use parameters
use kind_parameters
 implicit none

 integer :: imax, jmax
 integer :: ilevel
 integer :: it, jt

 do ilevel = num_level,1, -1

 imax = mg_level(ilevel)%nx
 jmax = mg_level(ilevel)%ny

! write(*,*) 'I am here'
! write(*,*) ilevel, imax, jmax

 ! Calculate control volumes

 mg_level(ilevel)%dx(1) = 0.0d0

 do it = 2,imax-1

  mg_level(ilevel)%dx(it) = Lx/(imax-2)

 end do

 mg_level(ilevel)%dx(imax) = 0.0d0

 mg_level(ilevel)%dy(1) = 0.0d0

 do jt = 2,jmax-1

  mg_level(ilevel)%dy(jt) = Ly/(jmax-2)

 end do

 mg_level(ilevel)%dy(jmax) = 0.0d0

 ! Calculate diffusion lengths

 mg_level(ilevel)%delx(1) = 0.5d0*mg_level(ilevel)%dx(2)

 do it = 2,imax-2

  mg_level(ilevel)%delx(it) = 0.5d0*(mg_level(ilevel)%dx(it)+mg_level(ilevel)%dx(it+1))

 end do

 mg_level(ilevel)%delx(imax-1) = 0.5d0*mg_level(ilevel)%dx(imax-1)

 mg_level(ilevel)%delx(0) =  mg_level(ilevel)%delx(1)
 mg_level(ilevel)%delx(-1) = mg_level(ilevel)%delx(2)
 mg_level(ilevel)%delx(-2) = mg_level(ilevel)%delx(3)
 mg_level(ilevel)%delx(imax) = mg_level(ilevel)%delx(imax-1)
 mg_level(ilevel)%delx(imax+1) = mg_level(ilevel)%delx(imax-2)
 mg_level(ilevel)%delx(imax+2) = mg_level(ilevel)%delx(imax-3)

 mg_level(ilevel)%dely(1) = 0.5d0*mg_level(ilevel)%dy(2)

 do jt = 2,jmax-2

  mg_level(ilevel)%dely(jt) = 0.5d0*(mg_level(ilevel)%dy(jt) + mg_level(ilevel)%dy(jt+1))

 end do

 mg_level(ilevel)%dely(jmax-1) = 0.5d0*mg_level(ilevel)%dy(jmax-1)

 mg_level(ilevel)%dely(0) =  mg_level(ilevel)%dely(1)
 mg_level(ilevel)%dely(-1) = mg_level(ilevel)%dely(2)
 mg_level(ilevel)%dely(-2) = mg_level(ilevel)%dely(3)
 mg_level(ilevel)%dely(jmax) = mg_level(ilevel)%dely(jmax-1)
 mg_level(ilevel)%dely(jmax+1) = mg_level(ilevel)%dely(jmax-2)
 mg_level(ilevel)%dely(jmax+2) = mg_level(ilevel)%dely(jmax-3)

! write(*,*) ''
! write(*,*) mg_level(ilevel)%dx
! write(*,*) ''
! write(*,*) mg_level(ilevel)%dy
! write(*,*) ''
! write(*,*) mg_level(ilevel)%delx
! write(*,*) ''
! write(*,*) mg_level(ilevel)%dely
! write(*,*) ''

 end do

end subroutine setup_mg_geometry


	real*4 function cosmol_c(h,omega,omega_lambda,q)

c	Returns cosmological constant = cosmol_c and parameter q

c       Omega is entered by the user
c       omega=1.-omega_lambda

c       Cosmological constant
        cosmol_c=omega_lambda/(3.*h**2)

c       Compute q=q0
        if (omega_lambda.eq.0.) then
                q=omega/2.
        else
                q=(3.*omega/2.) - 1.
        endif
	return
	end


      subroutine effectivescalelength(aeff)

      include 'commonblocks'

      sig2 = -psi0
      vb2 = v0bulge*v0bulge
      v02 = v0*v0
      if(v0bulge.ne.0.) then
         coef1 = 4./v0bulge/abulge/abulge*((vb2/sig2)**2.5)
      else
         coef1 = 0.
      endif
      coef2 = 1./v0/a/a*(((v02+2.*vb2)/sig2)**2.5)
      coef3 = sig2**1.5
      aeff = sqrt(4.*sig2/coef3/(coef1+coef2));
      return
      end

      subroutine dfcorrectionhalo(e,fac)

      include 'commonblocks'

      sig2 = -psi0
      call effectivescalelength(aeff)
c      fac = 1 + 3.*pi/4.*
c     +     ((bhmass/aeff/sig2)**1.5)/((1-e/sig2)**3.)

      del = 0.1/aeff
      den = sqrt((1-e/sig2)**2. + del**2.)
      fac = 1 + 3.*pi/4.*
     +     ((bhmass/aeff/sig2)**1.5)/(den**3.)
      return
      end

      subroutine dfcorrectionbulge(e,fac)

      include 'commonblocks'

      sig2 = -psi0
      call effectivescalelength(aeff)
      fac = 1 + 3.*pi/4.*
     +     ((bhmass/aeff/sig2)**1.5)/((1-e/sig2)**3.)
      return
      end

      function potbh(R,z)

      include 'commonblocks'

      potbh = - bhmass/sqrt(R*R + z*z)

      return
      end


      SUBROUTINE MB03VD( N, P, ILO, IHI, A, LDA1, LDA2, TAU, LDTAU,
     $                   DWORK, INFO )
C
C     SLICOT RELEASE 5.7.
C
C     Copyright (c) 2002-2020 NICONET e.V.
C
C     PURPOSE
C
C     To reduce a product of p real general matrices A = A_1*A_2*...*A_p
C     to upper Hessenberg form, H = H_1*H_2*...*H_p, where H_1 is
C     upper Hessenberg, and H_2, ..., H_p are upper triangular, by using
C     orthogonal similarity transformations on A,
C
C             Q_1' * A_1 * Q_2 = H_1,
C             Q_2' * A_2 * Q_3 = H_2,
C                    ...
C             Q_p' * A_p * Q_1 = H_p.
C
C     ARGUMENTS
C
C     Input/Output Parameters
C
C     N       (input) INTEGER
C             The order of the square matrices A_1, A_2, ..., A_p.
C             N >= 0.
C
C     P       (input) INTEGER
C             The number of matrices in the product A_1*A_2*...*A_p.
C             P >= 1.
C
C     ILO     (input) INTEGER
C     IHI     (input) INTEGER
C             It is assumed that all matrices A_j, j = 2, ..., p, are
C             already upper triangular in rows and columns 1:ILO-1 and
C             IHI+1:N, and A_1 is upper Hessenberg in rows and columns
C             1:ILO-1 and IHI+1:N, with A_1(ILO,ILO-1) = 0 (unless
C             ILO = 1), and A_1(IHI+1,IHI) = 0 (unless IHI = N).
C             If this is not the case, ILO and IHI should be set to 1
C             and N, respectively.
C             1 <= ILO <= max(1,N); min(ILO,N) <= IHI <= N.
C
C     A       (input/output) DOUBLE PRECISION array, dimension
C             (LDA1,LDA2,P)
C             On entry, the leading N-by-N-by-P part of this array must
C             contain the matrices of factors to be reduced;
C             specifically, A(*,*,j) must contain A_j, j = 1, ..., p.
C             On exit, the leading N-by-N upper triangle and the first
C             subdiagonal of A(*,*,1) contain the upper Hessenberg
C             matrix H_1, and the elements below the first subdiagonal,
C             with the first column of the array TAU represent the
C             orthogonal matrix Q_1 as a product of elementary
C             reflectors. See FURTHER COMMENTS.
C             For j > 1, the leading N-by-N upper triangle of A(*,*,j)
C             contains the upper triangular matrix H_j, and the elements
C             below the diagonal, with the j-th column of the array TAU
C             represent the orthogonal matrix Q_j as a product of
C             elementary reflectors. See FURTHER COMMENTS.
C
C     LDA1    INTEGER
C             The first leading dimension of the array A.
C             LDA1 >= max(1,N).
C
C     LDA2    INTEGER
C             The second leading dimension of the array A.
C             LDA2 >= max(1,N).
C
C     TAU     (output) DOUBLE PRECISION array, dimension (LDTAU,P)
C             The leading N-1 elements in the j-th column contain the
C             scalar factors of the elementary reflectors used to form
C             the matrix Q_j, j = 1, ..., P. See FURTHER COMMENTS.
C
C     LDTAU   INTEGER
C             The leading dimension of the array TAU.
C             LDTAU >= max(1,N-1).
C
C     Workspace
C
C     DWORK   DOUBLE PRECISION array, dimension (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
C     METHOD
C
C     The algorithm consists in ihi-ilo major steps. In each such
C     step i, ilo <= i <= ihi-1, the subdiagonal elements in the i-th
C     column of A_j are annihilated using a Householder transformation
C     from the left, which is also applied to A_(j-1) from the right,
C     for j = p:-1:2. Then, the elements below the subdiagonal of the
C     i-th column of A_1 are annihilated, and the Householder
C     transformation is also applied to A_p from the right.
C     See FURTHER COMMENTS.
C
C     REFERENCES
C
C     [1] Bojanczyk, A.W., Golub, G. and Van Dooren, P.
C         The periodic Schur decomposition: algorithms and applications.
C         Proc. of the SPIE Conference (F.T. Luk, Ed.), 1770, pp. 31-42,
C         1992.
C
C     [2] Sreedhar, J. and Van Dooren, P.
C         Periodic Schur form and some matrix equations.
C         Proc. of the Symposium on the Mathematical Theory of Networks
C         and Systems (MTNS'93), Regensburg, Germany (U. Helmke,
C         R. Mennicken and J. Saurer, Eds.), Vol. 1, pp. 339-362, 1994.
C
C     NUMERICAL ASPECTS
C
C     The algorithm is numerically stable.
C
C     FURTHER COMMENTS
C
C     Each matrix Q_j is represented as a product of (ihi-ilo)
C     elementary reflectors,
C
C        Q_j = H_j(ilo) H_j(ilo+1) . . . H_j(ihi-1).
C
C     Each H_j(i), i = ilo, ..., ihi-1, has the form
C
C        H_j(i) = I - tau_j * v_j * v_j',
C
C     where tau_j is a real scalar, and v_j is a real vector with
C     v_j(1:i) = 0, v_j(i+1) = 1 and v_j(ihi+1:n) = 0; v_j(i+2:ihi)
C     is stored on exit in A_j(i+2:ihi,i), and tau_j in TAU(i,j).
C
C     The contents of A_1 are illustrated by the following example
C     for n = 7, ilo = 2, and ihi = 6:
C
C     on entry                         on exit
C
C     ( a   a   a   a   a   a   a )    ( a   h   h   h   h   h   a )
C     ( 0   a   a   a   a   a   a )    ( 0   h   h   h   h   h   a )
C     ( 0   a   a   a   a   a   a )    ( 0   h   h   h   h   h   h )
C     ( 0   a   a   a   a   a   a )    ( 0   v2  h   h   h   h   h )
C     ( 0   a   a   a   a   a   a )    ( 0   v2  v3  h   h   h   h )
C     ( 0   a   a   a   a   a   a )    ( 0   v2  v3  v4  h   h   h )
C     ( 0   0   0   0   0   0   a )    ( 0   0   0   0   0   0   a )
C
C     where a denotes an element of the original matrix A_1, h denotes
C     a modified element of the upper Hessenberg matrix H_1, and vi
C     denotes an element of the vector defining H_1(i).
C
C     The contents of A_j, j > 1, are illustrated by the following
C     example for n = 7, ilo = 2, and ihi = 6:
C
C     on entry                         on exit
C
C     ( a   a   a   a   a   a   a )    ( a   h   h   h   h   h   a )
C     ( 0   a   a   a   a   a   a )    ( 0   h   h   h   h   h   h )
C     ( 0   a   a   a   a   a   a )    ( 0   v2  h   h   h   h   h )
C     ( 0   a   a   a   a   a   a )    ( 0   v2  v3  h   h   h   h )
C     ( 0   a   a   a   a   a   a )    ( 0   v2  v3  v4  h   h   h )
C     ( 0   a   a   a   a   a   a )    ( 0   v2  v3  v4  v5  h   h )
C     ( 0   0   0   0   0   0   a )    ( 0   0   0   0   0   0   a )
C
C     where a denotes an element of the original matrix A_j, h denotes
C     a modified element of the upper triangular matrix H_j, and vi
C     denotes an element of the vector defining H_j(i). (The element
C     (1,2) in A_p is also unchanged for this example.)
C
C     Note that for P = 1, the LAPACK Library routine DGEHRD could be
C     more efficient on some computer architectures than this routine
C     (a BLAS 2 version).
C
C     CONTRIBUTOR
C
C     V. Sima, Katholieke Univ. Leuven, Belgium, and A. Varga,
C     German Aerospace Center, DLR Oberpfaffenhofen, February 1999.
C     Partly based on the routine PSHESS by A. Varga
C     (DLR Oberpfaffenhofen), November 26, 1995.
C
C     REVISIONS
C
C     -
C
C     KEYWORDS
C
C     Hessenberg form, orthogonal transformation, periodic systems,
C     similarity transformation, triangular form.
C
C     ******************************************************************
C
C     .. Parameters ..
      DOUBLE PRECISION  ZERO
      PARAMETER         ( ZERO = 0.0D+0 )
C     ..
C     .. Scalar Arguments ..
      INTEGER           IHI, ILO, INFO, LDA1, LDA2, LDTAU, N, P
C     ..
C     .. Array Arguments ..
      DOUBLE PRECISION  A( LDA1, LDA2, * ), DWORK( * ), TAU( LDTAU, * )
C     ..
C     .. Local Scalars ..
      INTEGER           I, I1, I2, J, NH
C     ..
C     .. Local Arrays ..
      DOUBLE PRECISION  DUMMY( 1 )
C     ..
C     .. External Subroutines ..
      EXTERNAL          DCOPY, DLARFG, MB04PY, XERBLA
C     ..
C     .. Intrinsic Functions ..
      INTRINSIC         MAX, MIN
C     ..
C     .. Executable Statements ..
C
C     Test the input scalar arguments.
C
      INFO = 0
      IF( N.LT.0 ) THEN
         INFO = -1
      ELSE IF( P.LT.1 ) THEN
         INFO = -2
      ELSE IF( ILO.LT.1 .OR. ILO.GT.MAX( 1, N ) ) THEN
         INFO = -3
      ELSE IF( IHI.LT.MIN( ILO, N ) .OR. IHI.GT.N ) THEN
         INFO = -4
      ELSE IF( LDA1.LT.MAX( 1, N ) ) THEN
         INFO = -6
      ELSE IF( LDA2.LT.MAX( 1, N ) ) THEN
         INFO = -7
      ELSE IF( LDTAU.LT.MAX( 1, N-1 ) ) THEN
         INFO = -9
      END IF
      IF( INFO.NE.0 ) THEN
         CALL XERBLA( 'MB03VD', -INFO )
         RETURN
      END IF
C
C     Quick return if possible.
C
      NH = IHI - ILO + 1
      IF ( NH.LE.1 )
     $   RETURN
C
      DUMMY( 1 ) = ZERO
C
      DO 20 I = ILO, IHI - 1
         I1 = I + 1
         I2 = MIN( I+2, N )
C
         DO 10 J = P, 2, -1
C
C           Set the elements 1:ILO-1 and IHI:N-1 of TAU(*,J) to zero.
C
            CALL DCOPY( ILO-1, DUMMY, 0, TAU( 1, J ), 1 )
            IF ( IHI.LT.N )
     $         CALL DCOPY( N-IHI, DUMMY, 0, TAU( IHI, J ), 1 )
C
C           Compute elementary reflector H_j(i) to annihilate
C           A_j(i+1:ihi,i).
C
            CALL DLARFG( IHI-I+1, A( I, I, J ), A( I1, I, J ), 1,
     $                   TAU( I, J ) )
C
C           Apply H_j(i) to A_(j-1)(1:ihi,i:ihi) from the right.
C
            CALL MB04PY( 'Right', IHI, IHI-I+1, A( I1, I, J ),
     $                   TAU( I, J ), A( 1, I, J-1 ), LDA1, DWORK )
C
C           Apply H_j(i) to A_j(i:ihi,i+1:n) from the left.
C
            CALL MB04PY( 'Left', IHI-I+1, N-I, A( I1, I, J ),
     $                   TAU( I, J ), A( I, I1, J ), LDA1, DWORK )
   10    CONTINUE
C
C        Compute elementary reflector H_1(i) to annihilate
C        A_1(i+2:ihi,i).
C
         CALL DLARFG( IHI-I, A( I1, I, 1 ), A( I2, I, 1 ), 1,
     $                TAU( I, 1 ) )
C
C        Apply H_1(i) to A_p(1:ihi,i+1:ihi) from the right.
C
         CALL MB04PY( 'Right', IHI, IHI-I, A( I2, I, 1 ), TAU( I, 1 ),
     $                A( 1, I1, P ), LDA1, DWORK )
C
C        Apply H_1(i) to A_1(i+1:ihi,i+1:n) from the left.
C
         CALL MB04PY( 'Left', IHI-I, N-I, A( I2, I, 1 ), TAU( I, 1 ),
     $                A( I1, I1, 1 ), LDA1, DWORK )
   20 CONTINUE
C
      RETURN
C
C *** Last line of MB03VD ***
      END


      SUBROUTINE TG01MD( JOB, N, M, P, A, LDA, E, LDE, B, LDB, C, LDC,
     $                   ALPHAR, ALPHAI, BETA, Q, LDQ, Z, LDZ, NF, ND,
     $                   NIBLCK, IBLCK, TOL, IWORK, DWORK, LDWORK,
     $                   INFO )
C
C     SLICOT RELEASE 5.7.
C
C     Copyright (c) 2002-2020 NICONET e.V.
C
C     PURPOSE
C
C     To compute orthogonal transformation matrices Q and Z which
C     reduce the regular pole pencil A-lambda*E of the descriptor system
C     (A-lambda*E,B,C) to the form (if JOB = 'F')
C
C                ( Af  *  )             ( Ef  *  )
C       Q'*A*Z = (        ) ,  Q'*E*Z = (        ) ,                 (1)
C                ( 0   Ai )             ( 0   Ei )
C
C     or to the form (if JOB = 'I')
C
C                ( Ai  *  )             ( Ei  *  )
C       Q'*A*Z = (        ) ,  Q'*E*Z = (        ) ,                 (2)
C                ( 0   Af )             ( 0   Ef )
C
C     where the pair (Af,Ef) is in a generalized real Schur form, with
C     Ef nonsingular and upper triangular and Af in real Schur form.
C     The subpencil Af-lambda*Ef contains the finite eigenvalues.
C     The pair (Ai,Ei) is in a generalized real Schur form with
C     both Ai and Ei upper triangular. The subpencil Ai-lambda*Ei,
C     with Ai nonsingular and Ei nilpotent contains the infinite
C     eigenvalues and is in a block staircase form (see METHOD).
C
C     ARGUMENTS
C
C     Mode Parameters
C
C     JOB     CHARACTER*1
C             = 'F':  perform the finite-infinite separation;
C             = 'I':  perform the infinite-finite separation.
C
C     Input/Output Parameters
C
C     N       (input) INTEGER
C             The number of rows of the matrix B, the number of columns
C             of the matrix C and the order of the square matrices A
C             and E.  N >= 0.
C
C     M       (input) INTEGER
C             The number of columns of the matrix B.  M >= 0.
C
C     P       (input) INTEGER
C             The number of rows of the matrix C.  P >= 0.
C
C     A       (input/output) DOUBLE PRECISION array, dimension (LDA,N)
C             On entry, the leading N-by-N part of this array must
C             contain the N-by-N state matrix A.
C             On exit, the leading N-by-N part of this array contains
C             the transformed state matrix Q'*A*Z in the form
C
C             ( Af  *  )                    ( Ai  *  )
C             (        ) for JOB = 'F', or  (        )  for JOB = 'I',
C             ( 0   Ai )                    ( 0   Af )
C
C             where Af is an NF-by-NF matrix in real Schur form, and Ai
C             is an (N-NF)-by-(N-NF) nonsingular and upper triangular
C             matrix. Ai has a block structure as in (3) or (4), where
C             A0,0 is ND-by-ND and Ai,i , for i = 1, ..., NIBLCK, is
C             IBLCK(i)-by-IBLCK(i). (See METHOD.)
C
C     LDA     INTEGER
C             The leading dimension of the array A.  LDA >= MAX(1,N).
C
C     E       (input/output) DOUBLE PRECISION array, dimension (LDE,N)
C             On entry, the leading N-by-N part of this array must
C             contain the N-by-N descriptor matrix E.
C             On exit, the leading N-by-N part of this array contains
C             the transformed descriptor matrix Q'*E*Z in the form
C
C             ( Ef  *  )                    ( Ei  *  )
C             (        ) for JOB = 'F', or  (        )  for JOB = 'I',
C             ( 0   Ei )                    ( 0   Ef )
C
C             where Ef is an NF-by-NF nonsingular and upper triangular
C             matrix, and Ei is an (N-NF)-by-(N-NF) nilpotent matrix in
C             an upper triangular block form as in (3) or (4).
C
C     LDE     INTEGER
C             The leading dimension of the array E.  LDE >= MAX(1,N).
C
C     B       (input/output) DOUBLE PRECISION array, dimension (LDB,M)
C             On entry, the leading N-by-M part of this array must
C             contain the N-by-M input matrix B.
C             On exit, the leading N-by-M part of this array contains
C             the transformed input matrix Q'*B.
C
C     LDB     INTEGER
C             The leading dimension of the array B.  LDB >= MAX(1,N).
C
C     C       (input/output) DOUBLE PRECISION array, dimension (LDC,N)
C             On entry, the leading P-by-N part of this array must
C             contain the state/output matrix C.
C             On exit, the leading P-by-N part of this array contains
C             the transformed matrix C*Z.
C
C     LDC     INTEGER
C             The leading dimension of the array C.  LDC >= MAX(1,P).
C
C     ALPHAR  (output) DOUBLE PRECISION array, dimension (N)
C             ALPHAR(1:NF) will be set to the real parts of the diagonal
C             elements of Af that would result from reducing A and E to
C             the Schur form, and then further reducing both of them to
C             triangular form using unitary transformations, subject to
C             having the diagonal of E positive real.  Thus, if Af(j,j)
C             is in a 1-by-1 block (i.e., Af(j+1,j) = Af(j,j+1) = 0),
C             then ALPHAR(j) = Af(j,j). Note that the (real or complex)
C             values (ALPHAR(j) + i*ALPHAI(j))/BETA(j), j=1,...,NF, are
C             the finite generalized eigenvalues of the matrix pencil
C             A - lambda*E.
C
C     ALPHAI  (output) DOUBLE PRECISION array, dimension (N)
C             ALPHAI(1:NF) will be set to the imaginary parts of the
C             diagonal elements of Af that would result from reducing A
C             and E to Schur form, and then further reducing both of
C             them to triangular form using unitary transformations,
C             subject to having the diagonal of E positive real. Thus,
C             if Af(j,j) is in a 1-by-1 block (see above), then
C             ALPHAI(j) = 0. Note that the (real or complex) values
C             (ALPHAR(j) + i*ALPHAI(j))/BETA(j), j=1,...,NF, are the
C             finite generalized eigenvalues of the matrix pencil
C             A - lambda*E.
C
C     BETA    (output) DOUBLE PRECISION array, dimension (N)
C             BETA(1:NF) will be set to the (real) diagonal elements of
C             Ef that would result from reducing A and E to Schur form,
C             and then further reducing both of them to triangular form
C             using unitary transformations, subject to having the
C             diagonal of E positive real. Thus, if Af(j,j) is in a
C             1-by-1 block (see above), then BETA(j) = Ef(j,j).
C             Note that the (real or complex) values
C             (ALPHAR(j) + i*ALPHAI(j))/BETA(j), j=1,...,NF, are the
C             finite generalized eigenvalues of the matrix pencil
C             A - lambda*E.
C
C     Q       (output) DOUBLE PRECISION array, dimension (LDQ,N)
C             The leading N-by-N part of this array contains the
C             orthogonal matrix Q, which is the accumulated product of
C             the transformations applied to A, E, and B on the left.
C
C     LDQ     INTEGER
C             The leading dimension of the array Q.  LDQ >= MAX(1,N).
C
C     Z       (output) DOUBLE PRECISION array, dimension (LDZ,N)
C             The leading N-by-N part of this array contains the
C             orthogonal matrix Z, which is the accumulated product of
C             the transformations applied to A, E, and C on the right.
C
C     LDZ     INTEGER
C             The leading dimension of the array Z.  LDZ >= MAX(1,N).
C
C     NF      (output) INTEGER
C             The order of the reduced matrices Af and Ef; also, the
C             number of finite generalized eigenvalues of the pencil
C             A-lambda*E.
C
C     ND      (output) INTEGER
C             The number of non-dynamic infinite eigenvalues of the
C             matrix pair (A,E). Note: N-ND is the rank of the matrix E.
C
C     NIBLCK  (output) INTEGER
C             If ND > 0, the number of infinite blocks minus one.
C             If ND = 0, then NIBLCK = 0.
C
C     IBLCK   (output) INTEGER array, dimension (N)
C             IBLCK(i) contains the dimension of the i-th block in the
C             staircase form (3), where i = 1,2,...,NIBLCK.
C
C     Tolerances
C
C     TOL     DOUBLE PRECISION
C             A tolerance used in rank decisions to determine the
C             effective rank, which is defined as the order of the
C             largest leading (or trailing) triangular submatrix in the
C             QR factorization with column pivoting whose estimated
C             condition number is less than 1/TOL. If the user sets
C             TOL <= 0, then an implicitly computed, default tolerance
C             TOLDEF = N**2*EPS,  is used instead, where EPS is the
C             machine precision (see LAPACK Library routine DLAMCH).
C             TOL < 1.
C
C     Workspace
C
C     IWORK   INTEGER array, dimension (N)
C
C     DWORK   DOUBLE PRECISION array, dimension (LDWORK)
C             On exit, if INFO = 0, DWORK(1) returns the optimal value
C             of LDWORK.
C
C     LDWORK  INTEGER
C             The length of the array DWORK.  LDWORK >= 1, and if N > 0,
C             LDWORK >= 4*N.
C
C             If LDWORK = -1, then a workspace query is assumed; the
C             routine only calculates the optimal size of the DWORK
C             array, returns this value as the first entry of the DWORK
C             array, and no error message related to LDWORK is issued by
C             XERBLA.
C
C     Error Indicator
C
C     INFO    INTEGER
C             = 0:  successful exit;
C             < 0:  if INFO = -i, the i-th argument had an illegal
C                   value;
C             = 1:  the pencil A-lambda*E is not regular;
C             = 2:  the QZ iteration did not converge.
C
C     METHOD
C
C     For the separation of infinite structure, the reduction algorithm
C     of [1] is employed.
C     If JOB = 'F', the matrices Ai and Ei have the form
C
C           ( A0,0  A0,k ... A0,1 )         ( 0  E0,k ... E0,1 )
C      Ai = (  0    Ak,k ... Ak,1 ) ,  Ei = ( 0   0   ... Ek,1 ) ;   (3)
C           (  :     :    .    :  )         ( :   :    .    :  )
C           (  0     0   ... A1,1 )         ( 0   0   ...   0  )
C
C     if JOB = 'I' the matrices Ai and Ei have the form
C
C           ( A1,1 ... A1,k  A1,0 )         ( 0 ... E1,k  E1,0 )
C      Ai = (  :    .    :    :   ) ,  Ei = ( :  .    :    :   ) ,   (4)
C           (  :   ... Ak,k  Ak,0 )         ( : ...   0   Ek,0 )
C           (  0   ...   0   A0,0 )         ( 0 ...   0     0  )
C
C     where Ai,i, for i = 0, 1, ..., k, are nonsingular upper triangular
C     matrices. A0,0 corresponds to the non-dynamic infinite modes of
C     the system.
C
C     REFERENCES
C
C     [1] Misra, P., Van Dooren, P., and Varga, A.
C         Computation of structural invariants of generalized
C         state-space systems.
C         Automatica, 30, pp. 1921-1936, 1994.
C
C     NUMERICAL ASPECTS
C
C     The algorithm is numerically backward stable and requires
C     0( N**3 )  floating point operations.
C
C     FURTHER COMMENTS
C
C     The number of infinite poles is computed as
C
C                   NIBLCK
C        NINFP =     Sum  IBLCK(i) = N - ND - NF.
C                    i=1
C     The multiplicities of infinite poles can be computed as follows:
C     there are IBLCK(k)-IBLCK(k+1) infinite poles of multiplicity
C     k, for k = 1, ..., NIBLCK, where IBLCK(NIBLCK+1) = 0.
C     Note that each infinite pole of multiplicity k corresponds to
C     an infinite eigenvalue of multiplicity k+1.
C
C     CONTRIBUTOR
C
C     A. Varga, German Aerospace Center, DLR Oberpfaffenhofen.
C     July 1999. Based on the RASP routines SRISEP and RPDSGH.
C
C     REVISIONS
C
C     V. Sima, Dec. 2016.
C
C     KEYWORDS
C
C     Generalized eigenvalue problem, system poles, multivariable
C     system, orthogonal transformation, structural invariant.
C
C     ******************************************************************
C
C     .. Parameters ..
      DOUBLE PRECISION   ONE, ZERO
      PARAMETER          ( ONE = 1.0D0, ZERO = 0.0D0 )
C     .. Scalar Arguments ..
      CHARACTER          JOB
      INTEGER            INFO, LDA, LDB, LDC, LDE, LDQ, LDWORK, LDZ, M,
     $                   N, ND, NF, NIBLCK, P
      DOUBLE PRECISION   TOL
C     .. Array Arguments ..
      INTEGER            IBLCK( * ), IWORK(*)
      DOUBLE PRECISION   A(LDA,*), ALPHAI(*), ALPHAR(*), B(LDB,*),
     $                   BETA(*),  C(LDC,*),  DWORK(*),  E(LDE,*),
     $                   Q(LDQ,*), Z(LDZ,*)
C     .. Local Scalars ..
      LOGICAL            LQUERY, TRINF
      INTEGER            I, IHI, ILO, MINWRK, NBC, NC, NR, WRKOPT
C     .. Local Arrays ..
      DOUBLE PRECISION   DUM(1)
C     .. External Functions ..
      LOGICAL            LSAME
      EXTERNAL           LSAME
C     .. External Subroutines ..
      EXTERNAL           DGEMM, DHGEQZ, DLACPY, TG01LD, XERBLA
C     .. Intrinsic Functions ..
      INTRINSIC          INT, MAX, MIN
C     .. Executable Statements ..
C
C     Test the input parameters.
C
      INFO  =  0
      TRINF =  LSAME( JOB, 'I' )
      IF( .NOT.LSAME( JOB, 'F' ) .AND. .NOT.TRINF ) THEN
         INFO = -1
      ELSE IF( N.LT.0 ) THEN
         INFO = -2
      ELSE IF( M.LT.0 ) THEN
         INFO = -3
      ELSE IF( P.LT.0 ) THEN
         INFO = -4
      ELSE IF( LDA.LT.MAX( 1, N ) ) THEN
         INFO = -6
      ELSE IF( LDE.LT.MAX( 1, N ) ) THEN
         INFO = -8
      ELSE IF( LDB.LT.MAX( 1, N ) ) THEN
         INFO = -10
      ELSE IF( LDC.LT.MAX( 1, P ) ) THEN
         INFO = -12
      ELSE IF( LDQ.LT.MAX( 1, N ) ) THEN
         INFO = -17
      ELSE IF( LDZ.LT.MAX( 1, N ) ) THEN
         INFO = -19
      ELSE IF( TOL.GE.ONE ) THEN
         INFO = -24
      ELSE
         LQUERY = LDWORK.EQ.-1
         IF( N.EQ.0 ) THEN
            MINWRK = 1
         ELSE
            MINWRK = 4*N
         END IF
         IF( LQUERY ) THEN
            ILO = 1
            IHI = N
C
            CALL TG01LD( JOB, 'Hessenberg', 'Identity', 'Identity', N,
     $                   0, 0, A, LDA, E, LDE, DUM, LDB, DUM, LDC, Q,
     $                   LDQ, Z, LDZ, NF, ND, NIBLCK, IBLCK, TOL, IWORK,
     $                   DWORK, -1, INFO )
            WRKOPT = MAX( MINWRK, INT( DWORK(1) ) )
            CALL DHGEQZ( 'Schur', 'Vector', 'Vector', N, ILO, IHI, A,
     $                   LDA, E, LDE, ALPHAR, ALPHAI, BETA, Q, LDQ, Z,
     $                   LDZ, DWORK, -1, INFO )
            WRKOPT = MAX( WRKOPT, INT( DWORK(1) ) )
         END IF
         IF( LDWORK.LT.MINWRK .AND. .NOT.LQUERY ) THEN
            INFO = -27
         END IF
      END IF
C
      IF( INFO.NE.0 ) THEN
         CALL XERBLA( 'TG01MD', -INFO )
         RETURN
      ELSE IF( LQUERY ) THEN
         DWORK(1) = WRKOPT
         RETURN
      END IF
C
C     Quick return if possible
C
      IF( N.EQ.0 ) THEN
         NF = 0
         ND = 0
         NIBLCK   = 0
         DWORK(1) = ONE
         RETURN
      END IF
C
C     Compute the finite-infinite separation with A in Hessenberg form
C     and E upper triangular.
C     Workspace: need  4*N.
C
      CALL TG01LD( JOB, 'Hessenberg', 'Identity', 'Identity', N, 0, 0,
     $             A, LDA, E, LDE, DUM, LDB, DUM, LDC, Q, LDQ, Z, LDZ,
     $             NF, ND, NIBLCK, IBLCK, TOL, IWORK, DWORK, LDWORK,
     $             INFO )
C
      IF( INFO.NE.0 )
     $   RETURN
      WRKOPT = MAX( MINWRK, INT( DWORK(1) ) )
C
C     Reduce the finite part to generalized real Schur form.
C     Workspace: need  N;
C                prefer larger.
C
      IF( TRINF ) THEN
         ILO = N - NF + 1
         IHI = N
      ELSE
         ILO = 1
         IHI = NF
      END IF
      CALL DHGEQZ( 'Schur', 'Vector', 'Vector', N, ILO, IHI, A, LDA,
     $             E, LDE, ALPHAR, ALPHAI, BETA, Q, LDQ, Z, LDZ, DWORK,
     $             LDWORK, INFO )
      IF( INFO.NE.0 ) THEN
         INFO = 2
         RETURN
      END IF
      WRKOPT = MAX( WRKOPT, INT( DWORK(1) ) )
C
C     B <-- Q' * B.
C     Workspace: need   N;
C                prefer N*M.
C
      NBC = MAX( 1, MIN( LDWORK/N, M ) )
      DO 10 I = 1, M, NBC
         NC = MIN( NBC, M-I+1 )
         CALL DGEMM( 'Transpose', 'No transpose', N, NC, N, ONE, Q, LDQ,
     $               B(1,I), LDB, ZERO, DWORK, N )
         CALL DLACPY( 'All', N, NC, DWORK, N, B(1,I), LDB )
   10 CONTINUE
C
C     C <-- C * Z
C     Workspace: need   N;
C                prefer P*N.
C
      NBC = MAX( 1, MIN( LDWORK/N, P ) )
      DO 20 I = 1, P, NBC
         NR = MIN( NBC, P-I+1 )
         CALL DGEMM( 'No Transpose', 'No transpose', NR, N, N, ONE,
     $              C(I,1), LDC, Z, LDZ, ZERO, DWORK, NR )
         CALL DLACPY( 'All', NR, N, DWORK, NR, C(I,1), LDC )
   20 CONTINUE
C
      DWORK(1) = WRKOPT
C
      RETURN
C *** Last line of TG01MD ***
      END


!##############################################################################
! PROGRAM GridSearch2
!
! ## Value function iteration and a grid search speed-up
!
! This code is published under the GNU General Public License v3
!                         (https://www.gnu.org/licenses/gpl-3.0.en.html)
!
! Authors: Hans Fehr and Fabian Kindermann
!          contact@ce-fortran.com
!
! #VC# VERSION: 1.0  (23 January 2018)
!
!##############################################################################
include "prog08_04m.f90"

program GridSearch2

    use globals
    use toolbox

    implicit none

    ! start timer
    call tic()

    ! initialize a and value function
    call grid_Cons_Equi(a, 0d0, a0)
    V(:) = 0d0

    ! iterate until value function converges
    do iter = 1, itermax

        ! set a = 0 manually
        c(0) = 0d0
        ia_opt(0) = 0
        V_new(0) = V(1)-100d0

        ! calculate optimal decision for every gridpoint
        do ia = 1, NA

            ia_opt(ia) = max(ia-1, 0)
            cons = max(a(ia) - a(ia_opt(ia)), 1d-10)
            u_temp = cons**egam/egam + beta*V(ia_opt(ia))

            ! check whether there is an a_p that gives higher utility
            do ia_p = max(ia-2, 0), 0, -1

                ! calculate consumption and new utility
                cons = max(a(ia) - a(ia_p), 1d-10)
                u_new = cons**egam/egam + beta*V(ia_p)

                ! test which utility level is higher
                if(u_new > u_temp)then
                    ia_opt(ia) = ia_p
                    u_temp = u_new
                else
                    exit
                endif
            enddo

            ! get optimal consumption and value function
            c(ia) = a(ia)-a(ia_opt(ia))
            V_new(ia) = u_temp

        enddo

        ! get convergence level
        con_lev = maxval(abs(V_new(:) - V(:))/max(abs(V(:)), 1d-10))
        write(*,'(i5,2x,f20.7)')iter, con_lev

        ! check for convergence
        if(con_lev < sig)then
            call output()
        endif

        V = V_new
    enddo

    write(*,*)'No Convergence'

contains

    ! For creating output plots.
    subroutine output()

        use toolbox

        implicit none
        real*8 :: err, err_temp

        ! end timer
        call toc()

        ! calculate the time path of consumption numerically
        ia_t(0) = NA
        c_t(0) = c(NA)
        do it = 1, TT
            ia_t(it) = ia_opt(ia_t(it-1))
            c_t(it) = c(ia_t(it))
        enddo
        call plot((/(dble(it),it=0,TT)/), c_t, legend='numerical')

        ! calculate the time path of consumption analytically
        a_t(0) = a0
        c_t(0) = a_t(0)*(1d0-beta**gamma)
        do it = 1, TT
            a_t(it) = a_t(it-1) - c_t(it-1)
            c_t(it) = a_t(it)*(1d0-beta**gamma)
        enddo
        call plot((/(dble(it),it=0,TT)/), c_t, legend='analytical')
        call execplot(xlabel='Time t', ylabel='Consumption c_t')

        ! plot numerical and analytical consumption
        call plot(a, c, legend='numerical')
        call plot(a, a*(1d0-beta**gamma), legend='analytical')
        call execplot(xlabel='Level of resources a', ylabel='Policy Function c(a)')

        ! plot numerical and analytical value function
        call plot(a(10:NA), V(10:NA), legend='numerical')
        call plot(a(10:NA), (1d0-beta**gamma)**(-1d0/gamma)*a(10:NA) &
                                                **egam/egam, legend='analytical')
        call execplot(xlabel='Level of Resources a', ylabel='Value Function V(a)')

        ! calculate consumption function error
        err = 0d0
        do ia = 0, NA
            err_temp = abs(c(ia) - a(ia)*(1d0-beta**gamma)) &
                                /max(a(ia)*(1d0-beta**gamma), 1d-10)
            if(err_temp > err)err = err_temp
        enddo
        write(*,'(a, es15.7)')'Consumption function error:',err

        ! quit program
        stop

    end subroutine

end program


program ludic_numbers
  implicit none

  integer, parameter :: nmax = 25000
  logical :: ludic(nmax) = .true.
  integer :: i, j, n

  do i = 2, nmax / 2
    if (ludic(i)) then
      n = 0
      do j = i+1, nmax
        if(ludic(j)) n = n + 1
        if(n == i) then
          ludic(j) = .false.
          n = 0
        end if
      end do
    end if
  end do

  write(*, "(a)", advance = "no") "First 25 Ludic numbers: "
  n = 0
  do i = 1, nmax
    if(ludic(i)) then
      write(*, "(i0, 1x)", advance = "no") i
      n = n + 1
    end if
    if(n == 25) exit
  end do

  write(*, "(/, a)", advance = "no") "Ludic numbers below 1000: "
  write(*, "(i0)") count(ludic(:999))

  write(*, "(a)", advance = "no") "Ludic numbers 2000 to 2005: "
  n = 0
  do i = 1, nmax
    if(ludic(i)) then
       n = n + 1
       if(n >= 2000) then
         write(*, "(i0, 1x)", advance = "no") i
         if(n == 2005) exit
       end if
     end if
  end do

  write(*, "(/, a)", advance = "no") "Ludic Triplets below 250: "
  do i = 1, 243
    if(ludic(i) .and. ludic(i+2) .and. ludic(i+6)) then
       write(*, "(a, 2(i0, 1x), i0, a, 1x)", advance = "no") "[", i, i+2, i+6, "]"
    end if
  end do

end program


! ===================================================================
!   a Fortran routine to compute two point
!   angular correlation function
!   (c) Mehdi Rezaie, Sep 25, 2019
!
!   create the signature file (it is provided though)
!   to overwrite, use `--overwrite-signature'
!   > f2py ddthetahp.f95 -m ddthetahp -h ddthetahp.pyf
!   build final module
!   > f2py -c ddthetahp.pyf ddthetahp.f95
!
!   see test.py for an example
!
! ===================================================================
subroutine ddthetahpauto(theta1, phi1, delta1, fpix1, bins, C, n1, m)
    ! auto correlation function
    ! healpix based
    implicit none

    ! variables
    integer,intent(in)                  :: n1, m
    real(8),dimension(n1),intent(in)    :: theta1, phi1, delta1, fpix1
    real(8),dimension(m),intent(in)     :: bins
    real(8),dimension(2,m-1),intent(out) :: C

    ! local variables
    real(8) :: s, be, sinti, costi, sinpi, cospi, delta1i, fpix1i
    integer :: i,j,binid
    real(8), dimension(n1) :: cost1,sint1,cosp1,sinp1

    ! theta and phi
    cost1 = dcos(theta1)
    sint1 = dsin(theta1)
    cosp1 = dcos(phi1)
    sinp1 = dsin(phi1)

    C    = 0.0
    s    = 0.0

    do i = 1, n1
        ! temp array
        sinti   = sint1(i)
        costi   = cost1(i)
        sinpi   = sinp1(i)
        cospi   = cosp1(i)
        delta1i = delta1(i)
        fpix1i  = fpix1(i)

        do j = i+1, n1  ! no auto pairs
           s = sinti*sint1(j)*(cospi*cosp1(j) + sinpi*sinp1(j)) + (costi*cost1(j))
           be = bins(1) ! starts from the max sep angle
           binid = 0
           do while (s > be)
                binid = binid + 1
                be    = bins(binid+1)
           end do
           if ((binid .ge. 1) .and. (binid .le. m)) then
               C(1,binid) = C(1,binid) + delta1i*delta1(j)*fpix1i*fpix1(j)
               C(2,binid) = C(2,binid) + fpix1i*fpix1(j)
           end if
           s = 0
        end do
    end do

return
end subroutine ddthetahpauto

subroutine ddthetahpcross(theta1, phi1, delta1, fpix1, theta2, phi2, delta2, fpix2, bins, C, n1, n2, m)
    ! cross correlation function
    ! healpix based
    implicit none

    ! variables
    integer,intent(in)                  :: n1, n2, m
    real(8),dimension(n1),intent(in)    :: theta1, phi1, delta1, fpix1
    real(8),dimension(n2),intent(in)    :: theta2, phi2, delta2, fpix2
    real(8),dimension(m),intent(in)     :: bins
    real(8),dimension(2,m-1),intent(out) :: C

    ! local variables
    real(8) :: s, be, sinti, costi, sinpi, cospi, delta1i, fpix1i
    integer :: i,j,binid
    real(8), dimension(n1) :: cost1,sint1,cosp1,sinp1
    real(8), dimension(n2) :: cost2,sint2,cosp2,sinp2

    ! theta and phi
    cost1 = dcos(theta1)
    sint1 = dsin(theta1)
    cosp1 = dcos(phi1)
    sinp1 = dsin(phi1)

    cost2 = dcos(theta2)
    sint2 = dsin(theta2)
    cosp2 = dcos(phi2)
    sinp2 = dsin(phi2)

    C    = 0.0
    s    = 0.0

    do i = 1, n1
        ! temp array
        sinti   = sint1(i)
        costi   = cost1(i)
        sinpi   = sinp1(i)
        cospi   = cosp1(i)
        delta1i = delta1(i)
        fpix1i  = fpix1(i)

        do j = 1, n2  ! no auto pairs
           s = sinti*sint2(j)*(cospi*cosp2(j) + sinpi*sinp2(j)) + (costi*cost2(j))
           be = bins(1) ! starts from the max sep angle
           binid = 0
           do while (s > be)
                binid = binid + 1
                be    = bins(binid+1)
           end do
           if ((binid .ge. 1) .and. (binid .le. m)) then
               C(1,binid) = C(1,binid) + delta1i*delta2(j)*fpix1i*fpix2(j)
               C(2,binid) = C(2,binid) + fpix1i*fpix2(j)
           end if
           s = 0
        end do
    end do

return
end subroutine ddthetahpcross