*DECK CHFDV
      SUBROUTINE CHFDV (X1, X2, F1, F2, D1, D2, NE, XE, FE, DE, NEXT,
     +   IERR)
C***BEGIN PROLOGUE  CHFDV
C***PURPOSE  Evaluate a cubic polynomial given in Hermite form and its
C            first derivative at an array of points.  While designed for
C            use by PCHFD, it may be useful directly as an evaluator
C            for a piecewise cubic Hermite function in applications,
C            such as graphing, where the interval is known in advance.
C            If only function values are required, use CHFEV instead.
C***LIBRARY   SLATEC (PCHIP)
C***CATEGORY  E3, H1
C***TYPE      SINGLE PRECISION (CHFDV-S, DCHFDV-D)
C***KEYWORDS  CUBIC HERMITE DIFFERENTIATION, CUBIC HERMITE EVALUATION,
C             CUBIC POLYNOMIAL EVALUATION, PCHIP
C***AUTHOR  Fritsch, F. N., (LLNL)
C             Lawrence Livermore National Laboratory
C             P.O. Box 808  (L-316)
C             Livermore, CA  94550
C             FTS 532-4275, (510) 422-4275
C***DESCRIPTION
C
C        CHFDV:  Cubic Hermite Function and Derivative Evaluator
C
C     Evaluates the cubic polynomial determined by function values
C     F1,F2 and derivatives D1,D2 on interval (X1,X2), together with
C     its first derivative, at the points  XE(J), J=1(1)NE.
C
C     If only function values are required, use CHFEV, instead.
C
C ----------------------------------------------------------------------
C
C  Calling sequence:
C
C        INTEGER  NE, NEXT(2), IERR
C        REAL  X1, X2, F1, F2, D1, D2, XE(NE), FE(NE), DE(NE)
C
C        CALL  CHFDV (X1,X2, F1,F2, D1,D2, NE, XE, FE, DE, NEXT, IERR)
C
C   Parameters:
C
C     X1,X2 -- (input) endpoints of interval of definition of cubic.
C           (Error return if  X1.EQ.X2 .)
C
C     F1,F2 -- (input) values of function at X1 and X2, respectively.
C
C     D1,D2 -- (input) values of derivative at X1 and X2, respectively.
C
C     NE -- (input) number of evaluation points.  (Error return if
C           NE.LT.1 .)
C
C     XE -- (input) real array of points at which the functions are to
C           be evaluated.  If any of the XE are outside the interval
C           [X1,X2], a warning error is returned in NEXT.
C
C     FE -- (output) real array of values of the cubic function defined
C           by  X1,X2, F1,F2, D1,D2  at the points  XE.
C
C     DE -- (output) real array of values of the first derivative of
C           the same function at the points  XE.
C
C     NEXT -- (output) integer array indicating number of extrapolation
C           points:
C            NEXT(1) = number of evaluation points to left of interval.
C            NEXT(2) = number of evaluation points to right of interval.
C
C     IERR -- (output) error flag.
C           Normal return:
C              IERR = 0  (no errors).
C           "Recoverable" errors:
C              IERR = -1  if NE.LT.1 .
C              IERR = -2  if X1.EQ.X2 .
C                (Output arrays have not been changed in either case.)
C
C***REFERENCES  (NONE)
C***ROUTINES CALLED  XERMSG
C***REVISION HISTORY  (YYMMDD)
C   811019  DATE WRITTEN
C   820803  Minor cosmetic changes for release 1.
C   890411  Added SAVE statements (Vers. 3.2).
C   890531  Changed all specific intrinsics to generic.  (WRB)
C   890831  Modified array declarations.  (WRB)
C   890831  REVISION DATE from Version 3.2
C   891214  Prologue converted to Version 4.0 format.  (BAB)
C   900315  CALLs to XERROR changed to CALLs to XERMSG.  (THJ)
C***END PROLOGUE  CHFDV
C  Programming notes:
C
C     To produce a double precision version, simply:
C        a. Change CHFDV to DCHFDV wherever it occurs,
C        b. Change the real declaration to double precision, and
C        c. Change the constant ZERO to double precision.
C
C  DECLARE ARGUMENTS.
C
      INTEGER  NE, NEXT(2), IERR
      REAL  X1, X2, F1, F2, D1, D2, XE(*), FE(*), DE(*)
C
C  DECLARE LOCAL VARIABLES.
C
      INTEGER  I
      REAL  C2, C2T2, C3, C3T3, DEL1, DEL2, DELTA, H, X, XMI, XMA, ZERO
      SAVE ZERO
      DATA  ZERO /0./
C
C  VALIDITY-CHECK ARGUMENTS.
C
C***FIRST EXECUTABLE STATEMENT  CHFDV
      IF (NE .LT. 1)  GO TO 5001
      H = X2 - X1
      IF (H .EQ. ZERO)  GO TO 5002
C
C  INITIALIZE.
C
      IERR = 0
      NEXT(1) = 0
      NEXT(2) = 0
      XMI = MIN(ZERO, H)
      XMA = MAX(ZERO, H)
C
C  COMPUTE CUBIC COEFFICIENTS (EXPANDED ABOUT X1).
C
      DELTA = (F2 - F1)/H
      DEL1 = (D1 - DELTA)/H
      DEL2 = (D2 - DELTA)/H
C                                           (DELTA IS NO LONGER NEEDED.)
      C2 = -(DEL1+DEL1 + DEL2)
      C2T2 = C2 + C2
      C3 = (DEL1 + DEL2)/H
C                               (H, DEL1 AND DEL2 ARE NO LONGER NEEDED.)
      C3T3 = C3+C3+C3
C
C  EVALUATION LOOP.
C
      DO 500  I = 1, NE
         X = XE(I) - X1
         FE(I) = F1 + X*(D1 + X*(C2 + X*C3))
         DE(I) = D1 + X*(C2T2 + X*C3T3)
C          COUNT EXTRAPOLATION POINTS.
         IF ( X.LT.XMI )  NEXT(1) = NEXT(1) + 1
         IF ( X.GT.XMA )  NEXT(2) = NEXT(2) + 1
C        (NOTE REDUNDANCY--IF EITHER CONDITION IS TRUE, OTHER IS FALSE.)
  500 CONTINUE
C
C  NORMAL RETURN.
C
      RETURN
C
C  ERROR RETURNS.
C
 5001 CONTINUE
C     NE.LT.1 RETURN.
      IERR = -1
      CALL XERMSG ('SLATEC', 'CHFDV',
     +   'NUMBER OF EVALUATION POINTS LESS THAN ONE', IERR, 1)
      RETURN
C
 5002 CONTINUE
C     X1.EQ.X2 RETURN.
      IERR = -2
      CALL XERMSG ('SLATEC', 'CHFDV', 'INTERVAL ENDPOINTS EQUAL', IERR,
     +   1)
      RETURN
C------------- LAST LINE OF CHFDV FOLLOWS ------------------------------
      END


program integrateCube
	use cubeClass
	implicit none
	character(len=200)::input,output,integralOutput,cdfOutput
	type(cube)::cube1

	real(kind=8)::z,cdf,dx,dy,dz,bohrToAngstrom,integralToAngstrom
	integer::i,lastSlash,lastDot
	real(kind=8),allocatable,dimension(:)::integral

	call getarg(1,input)
	call getarg(2,output)

	cube1 = readCube(input)

	if (trim(output) == "")then
		lastSlash = scan(trim(input),"/", BACK= .true.)
		lastDot = scan(trim(input),".", BACK= .true.)
		if (lastDot == 0)then
			lastDot=len(input)
		else
			lastDot=lastDot-1
		end if
		integralOutput = trim(input(1:lastSlash))//'integral-'//trim(input(lastSlash+1:lastDot))//".dat"
		cdfOutput = trim(input(1:lastSlash))//"cdf-"//trim(input(lastSlash+1:lastDot))//".dat"
	else
		integralOutput="integral-"//trim(output)
		cdfOutput="cdf-"//trim(output)
	end if

	dx=cube1%axis_x(1)
	dy=cube1%axis_y(2)
	dz=cube1%axis_z(3)
	z=cube1%origin_z
	allocate(integral(cube1%nz))

	integral=sum(sum(cube1%values,1),1) * dx * dy

	open(unit=11,file=integralOutput,status='replace')
	open(unit=12,file=cdfOutput,status='replace')

	cdf = 0_8
	do i = 1, cube1%nz

		write(11,*) bohrToAngstrom(z),integralToAngstrom(integral(i))

		cdf = cdf + (integral(i) * dz)
		write(12,*) bohrToAngstrom(z),cdf

		z = z + dz
	end do

	close(11)
	close(12)

	deallocate(integral)

end program integrateCube

real(kind=8) function bohrToAngstrom(a)
	implicit none
	real(kind=8),intent(in)::a
	bohrToAngstrom = a * 0.529177210903_8
end function bohrToAngstrom

real(kind=8) function integralToAngstrom(a)
	implicit none
	real(kind=8),intent(in)::a
	integralToAngstrom = a / 0.529177210903_8
end function integralToAngstrom


submodule(forlab_linalg) forlab_linalg_svdsolve

    implicit none

contains

    module procedure svdsolve_sp
    integer :: i, k, n
    real(sp), dimension(:), allocatable :: w, xnorm, resnorm
    real(sp), dimension(:, :), allocatable :: U, V
    n = size(A, 2)
    k = n
    if (present(cutoff)) k = k - cutoff
    xnorm = zeros(n)
    resnorm = zeros(n)
    call svd(A, w, U, V)
    do i = 1, n
        x = matmul(matmul(matmul(V(:, :i), diag(1/w(:i))), transpose(U(:, :i))), b)
        xnorm(i) = norm(x)
        resnorm(i) = norm(matmul(A, x) - b)
    end do
    x = matmul(matmul(matmul(V(:, :k), diag(1/w(:k))), transpose(U(:, :k))), b)
    end procedure svdsolve_sp
    module procedure svdsolve_dp
    integer :: i, k, n
    real(dp), dimension(:), allocatable :: w, xnorm, resnorm
    real(dp), dimension(:, :), allocatable :: U, V
    n = size(A, 2)
    k = n
    if (present(cutoff)) k = k - cutoff
    xnorm = zeros(n)
    resnorm = zeros(n)
    call svd(A, w, U, V)
    do i = 1, n
        x = matmul(matmul(matmul(V(:, :i), diag(1/w(:i))), transpose(U(:, :i))), b)
        xnorm(i) = norm(x)
        resnorm(i) = norm(matmul(A, x) - b)
    end do
    x = matmul(matmul(matmul(V(:, :k), diag(1/w(:k))), transpose(U(:, :k))), b)
    end procedure svdsolve_dp

end submodule forlab_linalg_svdsolve


( Euler1 in Forth )

: Euler1 ( n -- r )
    0
    SWAP
    1 DO
        I 3 MOD 0= I 5 MOD 0= OR
        IF
            I + THEN
    LOOP ;

1000 Euler1
.
BYE


!***********************************************************************
!                                                                      *
      SUBROUTINE DRACAH(I, J, K, L, M, N, RAC)
!                                                                      *
!   SUBROUTINE  to calculate Racah coefficients. The arguments I, J,   *
!   K, L, M, N should be twice their actual value. Works for integer   *
!   and  half-integer  values of  angular momenta. The routine makes   *
!   use of the GAM  array, thus  SUBROUTINE FACTT must be called be-   *
!   fore this routine is used.                                         *
!                                                                      *
!   Written by N S Scott                    Last update: 16 Oct 1992   *
!   The last modification made by G. Gaigalas           October 2017   *
!                                                                      *
!***********************************************************************
!...Translated by Pacific-Sierra Research 77to90  4.3E  07:27:24   2/14/04
!-----------------------------------------------
!   M o d u l e s
!-----------------------------------------------
      USE vast_kind_param, ONLY:  DOUBLE
      USE FACTS_C
      IMPLICIT NONE
!-----------------------------------------------
!   D u m m y   A r g u m e n t s
!-----------------------------------------------
      INTEGER , INTENT(IN) :: I
      INTEGER , INTENT(IN) :: J
      INTEGER , INTENT(IN) :: K
      INTEGER , INTENT(IN) :: L
      INTEGER , INTENT(IN) :: M
      INTEGER , INTENT(IN) :: N
      REAL(DOUBLE) , INTENT(OUT) :: RAC
!-----------------------------------------------
!   L o c a l   V a r i a b l e s
!-----------------------------------------------
      INTEGER :: J1, J2, J3, J4, J5, J6, J7, NUMIN, NUMAX, ICOUNT, KK, KI
!-----------------------------------------------
      J1 = I + J + M
      J2 = K + L + M
      J3 = I + K + N
      J4 = J + L + N
      IF (2*MAX(MAX(I,J),M) - J1>0 .OR. MOD(J1,2)/=0) GO TO 2
      IF (2*MAX(MAX(K,L),M) - J2>0 .OR. MOD(J2,2)/=0) GO TO 2
      IF (2*MAX(MAX(I,K),N) - J3>0 .OR. MOD(J3,2)/=0) GO TO 2
      IF (2*MAX(MAX(J,L),N) - J4>0 .OR. MOD(J4,2)/=0) GO TO 2
      GO TO 1
    2 CONTINUE
      RAC = 0.0D00
      RETURN
!
    1 CONTINUE
      J1 = J1/2
      J2 = J2/2
      J3 = J3/2
      J4 = J4/2
      J5 = (I + J + K + L)/2
      J6 = (I + L + M + N)/2
      J7 = (J + K + M + N)/2
      NUMIN = MAX(MAX(MAX(J1,J2),J3),J4) + 1
      NUMAX = MIN(MIN(J5,J6),J7) + 1
      RAC = 1.0D00
      ICOUNT = 0
!
      IF (NUMIN /= NUMAX) THEN
         NUMIN = NUMIN + 1
!
         DO KK = NUMIN, NUMAX
            KI = NUMAX - ICOUNT
            RAC = 1.0D00 - RAC*DBLE(KI*(J5 - KI + 2)*(J6 - KI + 2)*(J7 - KI + 2&
               ))/DBLE((KI - 1 - J1)*(KI - 1 - J2)*(KI - 1 - J3)*(KI - 1 - J4))
            ICOUNT = ICOUNT + 1
         END DO
!
         NUMIN = NUMIN - 1
      ENDIF
      RAC = RAC*(-1.0D00)**(J5 + NUMIN + 1)*EXP((GAM(NUMIN+1)-GAM(NUMIN-J1)-GAM&
         (NUMIN-J2)-GAM(NUMIN-J3)-GAM(NUMIN-J4)-GAM(J5+2-NUMIN)-GAM(J6+2-NUMIN)&
         -GAM(J7+2-NUMIN))+(GAM(J1+1-I)+GAM(J1+1-J)+GAM(J1+1-M)-GAM(J1+2)+GAM(&
         J2+1-K)+GAM(J2+1-L)+GAM(J2+1-M)-GAM(J2+2)+GAM(J3+1-I)+GAM(J3+1-K)+GAM(&
         J3+1-N)-GAM(J3+2)+GAM(J4+1-J)+GAM(J4+1-L)+GAM(J4+1-N)-GAM(J4+2))*&
         0.5D00)
!
      RETURN
      END SUBROUTINE DRACAH


C NCLFORTSTART
      subroutine dmlegevi(x, nx, xmsg, vals, xx, ierr)
      implicit none
C                                               !  INPUT
      integer nx
      double precision x(nx),  xx(nx), xmsg
C                                               !  OUTPUT
      integer ierr
      double precision vals(6)
C NCLEND
c
c Interface to AS-215 algorithm
c     (1) remove msg values
c     (2) create 1st guess for parameters
c     (2) return a selected subset of values
c                                               ! LOCAL
      integer monit, nxn, n
      double precision para(3), vcov(6)         ! see subroutine
      double precision euler, pi, six, zero     ! local variables
      double precision x1, x2, x3, xvar, xstd, xavg

c c c double precision xx(nx)                   ! f90 auto array
c c c double precision,allocatable::xx(:)       ! allocate in calling routine

      pi    = 3.141592653589793d0
      euler = 0.577215664901532d0        ! Euler–Mascheroni constant
      six   = 6.0d0
      zero  = 0.0d0
c                                               ! strip msg
c     allocate(xx(nx))
      nxn = 0
      do n=1,nx
         if (x(n).ne.xmsg) then
             nxn = nxn+1
             xx(nxn) = x(n)
         end if
      end do

c c c if (nxn.eq.0) then
      if (nxn.lt.10) then                       ! arbitrary
          do n=1,6
             vals(n) = xmsg
          end do
          ierr  = -999                          ! all msg
      else
          x1  = zero
          x2  = zero
          do n=1,nxn
             x1 = x1 + xx(n)
             x2 = x2 + xx(n)*xx(n)
          end do
          x3    = x1*x1/nxn
          xvar  = (x2-x3)/(nxn-1)
          xstd  = sqrt(max(xvar,zero))          ! prevent possible roundoff
          xavg  = x1/nxn
                                                ! 1st guess at parameters
          para(1) = zero                        ! location, center, shift
          para(2) = xstd*sqrt(six/pi)           ! scale
          para(3) = xavg-euler*para(1)          ! shape

          monit = 0                             ! no monitoring
          call dmlegev(xx, nxn, para, vcov, monit, ierr)

          vals(1) = para(1)                     ! location (center, shift)
          vals(2) = para(2)                     ! scale parameter
          vals(3) = para(3)                     ! shape parameter

c                                               ! std errors
          vals(4) = sqrt(vcov(1))               ! location (center, shift)
          vals(5) = sqrt(vcov(3))               ! scale
          vals(6) = sqrt(vcov(6))               ! shape
      end if
      return
      end
C-----------------------------------------------------------------
c Algorithm AS 215
c Maximum-Likelihood Estimation of the Parameters of the Generalized Extreme-Value Distribution
c J. R. M. Hosking
c Journal of the Royal Statistical Society. Series C (Applied Statistics)
c Vol. 34, No. 3 (1985), pp. 301-310
c Stable URL: http://www.jstor.org/stable/2347483
C-----------------------------------------------------------------
      SUBROUTINE DMLEGEV(X, N, PARA, VCOV, MONIT, IFAULT)
      IMPLICIT NONE
      INTEGER N, MONIT, IFAULT
      DOUBLE PRECISION PARA(3), VCOV(6), X(N)

C CODE:    http://ftp.uni-bayreuth.de/math/statlib/apstat/215
C ARTICLE: http://www.jstor.org/stable/pdf/2347483

c INPUT
c  X  Double precision array (N) input: data array
c  N Integer input: sample size
c  PARA Double precision array (3) input: initial estimates of the parameters, in the order #, ae, k
c
C OUTPUT
C VCOV(6) double precision array (6): on successful exit (IFAULT= 0)
C         the lower triangle of the estimated variance-covariance matrix, H-1,
C         of the maximum-likelihood estimates, stored row by row.
C MONIT  Integer input: flag for monitoring of iterations (see below)
C IFAULT Integer output: failure indicator (see below)
C      0 on successful exit
C      1 if N < 2. PARA and VCO V are set to zero
C      2 if maximum number of iterations reached
C      3 if maximum number of evaluations of log-likelihood reached
C      4 if maximum number of steplength reductions reache
C
      INTEGER I, ITYPE, KK, MAXEV, MAXIT, MAXSR, NEVAL, NITER, NSR

C        ALGORITHM AS215   APPL. STATIST. (1985) VOL. 34, NO. 3
C        Modifications in AS R76 (1989) have been incorporated.
C
C        Additional modifications by J. R. M. Hosking, August 1994:
C        Modify steepest-ascent step so that it is invariant to
C        rescaling the data.  Improves chance of convergence from
C        poor initial values.
C
C        MAXIMUM-LIKELIHOOD ESTIMATION OF GENERALIZED EXTREME-VALUE
C        DISTRIBUTION
C
      DOUBLE PRECISION A, ACCA, ACCG, ACCU, AI, AIGI, AN, D, DA, DAA,
     *  DAG, DELA, DELG, DELU, DG, DGG, DU, DUA, DUG, DUU, E, F, FOLD,
     *  G, GAI, GG, GI, GIPQ, GNORM, H, HALF, HE, HH, ONE, P, PA, PQ,
     *  PQG, PU, Q, QA, QU, R, RA, RATIO, RG, RU, SE, SH, SHE, SHH,
     *  SHHE, SMALL, SRF, STEPA, STEPG, STEPU, SY, SYE, SYHE, SYYE,
     *  TEMP1, TEMP2, U, VLNEG, XMAX, XMIN, Y, YE, Z, ZERO
      CHARACTER*8 ACTI1, ACTI2, ACTI3, ACTI4, ACTI5, ACTI6, ACTI7,
     *  ACTI8, ACTI9
      DATA ACTI1/'  NEWTON'/, ACTI2/'  ST.ASC'/, ACTI3/'  RESETK'/,
     *  ACTI4/'  SR.INF'/, ACTI5/'  SR.LIK'/, ACTI6/'  MAX.SR'/,
     *  ACTI7/'  MAX.EV'/, ACTI8/'  MAX.IT'/, ACTI9/'  CONVGD'/
      DATA ZERO /0.0D0/, HALF /0.5D0/, ONE/1.0D0/
C
C        ADDU,ACCA,ACCG ARE ACCURACY CRITERIA FOR TESTING CONVERGENCE
C        STEPU,STEPA,STEPG ARE MAXIMUM STEPLENGTHS FOR ITERATIONS
C        ACCU,ACCA,STEPU,STEPA ARE SCALED BY CURRENT VALUE OF A WHEN
C        USED IN PROGRAM
C
      DATA ACCU, ACCA, ACCG /3 * 1D-5/, STEPU, STEPA, STEPG /
     *  0.5D0, 0.25D0, 0.2D0/
C
C        MAXIT IS MAX. NO. OF ITERATIONS
C        MAXEV IS MAX. NO. OF EVALUATIONS OF LIKELIHOOD FUNCTION
C        SRF IS STEPLENGTH REDUCTION FACTOR
C        MAXSR IS MAX. NO. OF STEPLENGTH REDUCTIONS PERMITTED PER
C        ITERATION
C
      DATA MAXIT /30/, MAXEV /50/, SRF /0.25D0/, MAXSR /30/
C
C        SMALL IS A SMALL NUMBER, USED TO ADJUST THE SHAPE PARAMETER TO
C        AVOID AN EXACT ZERO VALUE OR BORDERLINE INFEASIBILITY
C        ALNEG IS A LARGE NEGATIVE NUMBER, USED TO INITIALIZE
C        LOG-LIKELIHOOD
C
      DATA SMALL /1.D-3/, VLNEG /-1.D37/
C
C        FIND MIN AND MAX DATA VALUE
C
      DO 10 I = 1, 6
  10  VCOV(I) = ZERO
      IFAULT = 1
      IF (N .LE. 2) GOTO 170
      XMIN = X(1)
      XMAX = X(1)
      DO 20 I = 2, N
        IF (X(I) .LT. XMIN) XMIN = X(I)
        IF (X(I) .GT. XMAX) XMAX = X(I)
   20 CONTINUE
C
C       INITIALIZATION
C       U IS LOCATION PARAMETER
C       A IS SCALE PARAMETER
C       G IS SHAPE PARAMETER
C
      IF (MONIT .GT. 0) WRITE (6, 6000)
      IFAULT = 0
      NITER = 0
      NEVAL =0
      FOLD = VLNEG
      U = PARA(1)
      A = PARA(2)
      G = PARA(3)
      IF (ABS(G) .LT. SMALL) G = SMALL
      IF (A .LE. ZERO) A = ONE
      AN = DBLE(FLOAT(N))
C
C        CHECK WHETHER ALL DATA POINTS LIE WITHIN THE RANGE OF THE GEV
C        DISTRIBUTION WITH THE INITIAL PARAMETERS - IF NOT, ADJUST THE
C        SHAPE PARAMETER SO AS TO BRING ALL POINTS WITHIN RANGE
C
      IF (G .GT. ZERO) GOTO 30
      IF (XMIN .GE. U) GOTO 40
      Z = A / (XMIN - U)
      IF (G .GT. Z) GOTO 40
      IF (MONIT .GT. 0) WRITE (6, 6010) NITER, NEVAL, U, A, G, ACTI3
      G = Z + SMALL
      IF (G .GE. ZERO) G = HALF * Z
      GOTO 40
   30 IF (XMAX .LE. U) GOTO 40
      Z = A / (XMAX - U)
      IF (G .LT. Z) GOTO 40
      IF(MONIT .GT. 0) WRITE (6, 6010) NITER, NEVAL, U, A, G, ACTI3
      G = Z - SMALL
      IF (G .LE. ZERO) G = HALF * Z
C
C        START OF MAIN LOOP
C
   40 DO 140 NITER = 1, MAXIT
C
      NSR = 0
   50 IF (NEVAL .GE. MAXEV) GOTO 150
      NEVAL = NEVAL + 1
      AI = ONE / A
      GI = ONE /G
      GAI = G * AI
      AIGI = AI * GI
      GG = ONE - G
C
C       ACCUMULATE SUMS OF QUANTITIES OCCURRING IN LIKELIHOOD
C       DERIVATIVES
C
C       IN PRESCOTT AND WALDEN'S NOTATION:
C       Z IS 1 - K * (X(I)-U) / A
C       Y IS THE REDUCED VARIATE - (1/K) * LOG(Z)
C       E IS EXP(-Y)
C       H IS EXP(K*Y)
C
      SY = ZERO
      SE = ZERO
      SYE = ZERO
      SYYE = ZERO
      SH = ZERO
      SHE = ZERO
      SYHE = ZERO
      SHHE = ZERO
      SHH = ZERO
      DO 60 I = 1, N
        Z = ONE -GAI * (X(I) - U)
        Y = -GI * LOG(Z)
        E = EXP(-Y)
        H = ONE / Z
        YE = Y * E
        HE = H * E
        HH = H * H
        SY = SY + Y
        SE = SE + E
        SYE = SYE + YE
        SYYE = SYYE + Y * YE
        SH = SH + H
        SHE = SHE + HE
        SYHE = SYHE + Y * HE
        SHHE = SHHE + HH * E
        SHH = SHH + HH
   60 CONTINUE
C
C       F IS CURRENT VALUE OF LIKELIHOOD FUNCTIONN
C
      F = -AN * LOG(A) - GG * SY - SE
      IF (F .GT. FOLD) GOTO 90
C
C       LIKELIHOOD HAS NOT INCREASED - REDUCE STEPLENGTH AND TRY AGAIN
C
      IF (MONIT .GT. 0) WRITE(6, 6010) NITER, NEVAL, U, A, G, ACTI5, F
      IF (NSR .EQ. MAXSR) GOTO 80
   70 NSR = NSR + 1
      U = U - DELU
      A = A - DELA
      G = G - DELG
      DELU = SRF * DELU
      DELA = SRF * DELA
      DELG = SRF * DELG
      U = U + DELU
      A = A + DELA
      G = G + DELG
      IF (A .GT. G * (XMIN - U) .AND. A .GT. G * (XMAX - U) .AND. G .NE.
     *  ZERO) GO TO 50
      IF (MONIT .GT. 0) WRITE (6, 6010) NITER, NEVAL, U, A, G, ACTI4
      IF (NSR .LT. MAXSR) GOTO 70
C
C        MAX. NO. OF STEPLENGTH REDUCTIONS REACHED
C        IF CURRENT ITERATION IS NEWTON-RAPHSON, TRY STEEPEST ASCENT
C        INSTEAD.  IF CURRENT ITERATION IS STEEPEST ASCENT, GIVE UP.
C
   80 U = U - DELU
      A = A - DELA
      G = G - DELG
      IF (MONIT .GT. 0) WRITE(6, 6010) NITER, NEVAL, U, A, G, ACTI6
      IF (ITYPE .EQ. 1) GOTO 100
      IFAULT = 4
      GOTO 160
C
C        P,Q,R, ARE AS DEFINED IN FLOOD STUDIES REPORT
C
   90 FOLD = F
      P = AN - SE
      Q = SHE - GG * SH
      R = AN - SY + SYE
      PQ = P + Q
      GIPQ = GI * PQ
C
C        FIRST DERIVATIVES OF LOG-LIKELIHOOD
C
      DU = -AI * Q
      DA = -AIGI * PQ
      DG = -GI * (R - GIPQ)
      IF (MONIT .GT. 0) GNORM = SQRT(DU * DU + DA * DA + DG * DG)
C
C        DERIVATIVES OF P,Q,R
C
      PU = -AI * SHE
      PA = GI * PU + AIGI * SE
      QU = GG * AI * (SHHE + G * SHH)
      RU = AI * (SH - SHE + SYHE)
      RA = GI * RU - AIGI * (AN - SE + SYE)
      RG = GI * (SY - SYE + SYYE - A * RA)
      QA = AI * Q + GI * (PU + QU)
      PQG = GIPQ + A * (RA - GI * (PA + QA))
C
C         MINUS SECOND DERIVATIVE OF LOG-LIKELIHOOD (HESSIAN MATRIX)
C
      DUU = AI * QU
      DUA = AIGI * (PU + QU)
      DAA = -AIGI * (AI * PQ - PA - QA)
      DUG = GI * (RU - GI * (PU + QU))
      DAG = -AIGI * (GIPQ - PQG)
      DGG = GI * (RG - GI * (PQG + R - GIPQ - GIPQ))
C
C         INVERT HESSIAN MATRIX
C
      DO 95 KK = 1, 3
        IF (DUU .LE. ZERO) GO TO 100
        D = ONE / DUU
        TEMP1 = -DUA * D
        IF (KK .GT. 2) TEMP1 = -TEMP1
        TEMP2 = -DUG * D
        IF (KK .GT. 1) TEMP2 = -TEMP2
        DUU = DAA + TEMP1 * DUA
        DUA = DAG + TEMP1 * DUG
        DAA = DGG + TEMP2 * DUG
        DUG = TEMP1
        DAG = TEMP2
        DGG = D
   95 CONTINUE
C
C        CALCULATE STEPLENGTHS
C
      ITYPE = 1
      IF (MONIT .GT. 0) WRITE(6, 6010) NITER, NEVAL, U, A, G, ACTI1, F,
     *  GNORM
      DELU = DUU * DU + DUA * DA + DUG * DG
      DELA = DUA * DU + DAA * DA + DAG * DG
      DELG = DUG * DU + DAG * DA + DGG * DG
      RATIO = MAX(ABS(DELU) / (STEPU * A), ABS(DELA) / (STEPA * A),
     *  ABS(DELG) / STEPG)
      IF (RATIO .LT. ONE) GOTO 110
      RATIO = ONE / RATIO
      DELU = DELU * RATIO
      DELA = DELA * RATIO
      DELG = DELG * RATIO
      GOTO 110
C
C        HESSIAN IS NOT POSITIVE DEFINITE - MAKE A LARGE STEP IN THE
C        DIRECTION OF STEEPEST ASCENT
C
  100 ITYPE = 2
      IF (MONIT .GT. 0) WRITE(6, 6010) NITER, NEVAL, U, A, G, ACTI2, F,
     *  GNORM
C---------------------------------------  NEXT 11 LINES ADDED, AUG.94
      D = ABS(VLNEG)
      TEMP1 = D
      IF (DU .NE. ZERO) TEMP1 = STEPU / (ABS(DU) * A)
      TEMP2 = D
      IF (DA .NE. ZERO) TEMP2 = STEPA / (ABS(DA) * A)
      Z = D
      IF (DG .NE. ZERO) Z = STEPG / ABS(DG)
      RATIO = MIN(TEMP1, TEMP2, Z)
      DELU = RATIO * DU * A * A
      DELA = RATIO * DA * A * A
      DELG = RATIO * DG
C---------------------------------------  NEXT 11 LINES REMOVED, AUG.94
C      D = ABS(VLNEG)
C      TEMP1 = D
C      IF (DU .NE. ZERO) TEMP1 = STEPU * A / ABS(DU)
C      TEMP2 = D
C      IF (DA .NE. ZERO) TEMP2 = STEPA * A / ABS(DA)
C      Z = D
C      IF (DG .NE. ZERO) Z = STEPG / ABS(DG)
C      RATIO = MIN(TEMP1, TEMP2, Z)
C      DELU = RATIO * DU
C      DELA = RATIO * DA
C      DELG = RATIO * DG
C---------------------------------------  END OF DELETED CODE
C
C        ADJUST PARAMETERS
C
  110 U = U + DELU
      A = A + DELA
      G = G + DELG
C
C        TEST FOR FEASIBILITY
C
      IF (A .GT. G * (XMIN - U) .AND. A .GT. G * (XMAX - U) .AND. G .NE.
     *  ZERO) GOTO 130
      DO 120 NSR = 1, MAXSR
        IF (MONIT .GT. 0) WRITE(6, 6010) NITER, NEVAL, U, A, G, ACTI4
        U = U - DELU
        A = A - DELA
        G = G - DELG
        DELU = SRF * DELU
        DELA = SRF * DELA
        DELG = SRF * DELG
        U = U + DELU
        A = A + DELA
        G = G + DELG
        IF (A .GT. G * (XMIN - U) .AND. A .GT. G * (XMAX - Y) .AND.
     *     G .NE. ZERO) GOTO 140
  120 CONTINUE
      GOTO 80
C
C        TEST FOR CONVERGENCE
C
  130 IF (ABS(DELU) .GT. ACCU * A) GOTO 140
      IF (ABS(DELA) .GT. ACCA * A) GOTO 140
      IF (ABS(DELG) .GT. ACCG) GOTO 140
      IF (MONIT .GT. 0) WRITE(6, 6010) NITER, NEVAL, U, A, G, ACTI9
      VCOV(1) = DUU
      VCOV(2) = DUA
      VCOV(3) = DAA
      VCOV(4) = DUG
      VCOV(5) = DAG
      VCOV(6) = DGG
      GOTO 160
C
C        END OF MAIN LOOP
C
  140 CONTINUE
C
C        ITERATIONS NOT CONVERGED - SET FAULT FLAG
C
      IFAULT = 2
      IF (MONIT .GT. 0) WRITE(6, 6010) MAXIT, NEVAL, U, A, G, ACTI8
      GOTO 160
  150 IFAULT = 3
      IF (MONIT .GT. 0) WRITE(6, 6010) NITER, MAXEV, U, A, G, ACTI7
C
C        ITERATION FINISHED -COPY RESULTS INTO ARRAY PARA
C
  160 IF (MONIT .GT. 0) WRITE(6, 6020)
      PARA(1) = U
      PARA(2) = A
      PARA(3) = G
      RETURN
C
  170 DO 180 I = 1, 3
  180 PARA(I) = ZERO
      RETURN
C
 6000 FORMAT(/' MAXIMUM-LIKELIHOOD ESTIMATION OF GENERALIZED EXTREME',
     *  1X, 'VALUE DISTRIBUTION'// ' ITER EVAL', 8X, 'XI', 5X, 'ALPHA',
     *  9X, 'K  ACTION', 6X, 'LOG-L', 7X, 'GNORM')
 6010 FORMAT(1X, I4, I5, 3F10.4, 1X, A, F11.3, 1PD12.2)
 6020 FORMAT(1H0)
      END


!---------------------------------------------------!
! Copyright (c) 2017 Shunsuke A. Sato               !
! Released under the MIT license                    !
! https://opensource.org/licenses/mit-license.php   !
!---------------------------------------------------!
!-------10--------20--------30--------40--------50--------60--------70--------80--------90
subroutine set_deps_2d
  use global_variables_2d
  implicit none
  integer :: ikx,iky
  real(8) :: dipx,dipy, mu_y

  select case(nband_type)
  case(N_PARABOLIC_BAND)
!$omp parallel do private(ikx, iky) collapse(2)
    do ikx = -NKx,NKx
      do iky = -NKy,NKy
        deps(ikx,iky) = eps_g + 0.5d0/mass_r*(kx(ikx)**2+ky(iky)**2)
      end do
    end do
  case(N_COS_BAND)
!$omp parallel do private(ikx, iky) collapse(2)
    do ikx = -NKx,NKx
      do iky = -NKy,NKy
        deps(ikx,iky) = eps_g - 0.5d0*band_width*(&
          cos(2d0*pi*kx(ikx)/kx_max)*cos(2d0*pi*ky(iky)/ky_max) -1d0)
      end do
    end do
  case(N_COS4_BAND)
!$omp parallel do private(ikx, iky) collapse(2)
    do ikx = -NKx,NKx
      do iky = -NKy,NKy
        deps(ikx,iky) = eps_g +band_width*&
          (1d0 - cos(pi*kx(ikx)/kx_max)**4*cos(pi*ky(iky)/ky_max)**4)
      end do
    end do
  case(N_HBN_BAND)
    dipx = 2d0*pi/kx_max
    mu_y = 2d0/(dipx**2*band_width)
!$omp parallel do private(ikx, iky) collapse(2)
    do ikx = -NKx,NKx
      do iky = -NKy,NKy
        deps(ikx,iky) = eps_g &
          +0.5d0*band_width*(1d0 - cos(dipx*kx(ikx))) &
          +0.5d0*ky(iky)**2/mu_y
      end do
    end do
  case default
    write(*,"(A,2x,A)")"Invalid nband_type",nband_type
    stop
  end select

  return
end subroutine set_deps_2d


      subroutine gdd(dgpit,fxi,gpit,ipbtmx,napmax,napt,palpha)
c
c     This subroutine computes the functions g(xi) and their ionic
c     strength derivatives (xi = alpha(i) * I**1/2). These functions
c     are used in Pitzer's equations. Note: alpha(i) = 0 implies
c     that g(xi) and its derivatives are zero.
c
c     This subroutine is called by:
c
c       EQLIBG/gcoeff.f
c
c-----------------------------------------------------------------------
c
c     Principal input:
c
c       fxi    = the ionic strength (the 2nd-order electrostatic
c                  moment function I)
c       napt   = number of alpha coefficient pairs in the palpha array
c
c     Principal output:
c
c       gpit   = array of values for the g(x) function
c       dgpit  = array of values for the ionic strength derivatives
c                  of the g(x) function
c
c-----------------------------------------------------------------------
c
      implicit none
c
c-----------------------------------------------------------------------
c
c     Calling sequence variable declarations.
c
      integer ipbtmx,napmax
c
      integer napt
c
      real(8) dgpit(2,ipbtmx,napmax),gpit(ipbtmx,napmax),
     $ palpha(ipbtmx,napmax)
      real(8) fxi
c
c-----------------------------------------------------------------------
c
c     Local variable declarations.
c
      integer i,nap
c
      real(8) alphai,dxdi,emx,gpx,gppx,x,xsq
c
c-----------------------------------------------------------------------
c
      do nap = 1,napt
        do i = 1,ipbtmx
          gpit(i,nap) = 0.
          dgpit(1,i,nap) = 0.
          dgpit(2,i,nap) = 0.
          alphai = palpha(i,nap)
          x = alphai * sqrt(fxi)
c
          if (x .ne. 0.) then
            xsq = x*x
            emx = exp(-x)
c
c           Calculate g(x).
c
            gpit(i,nap) = (2./xsq) * (1. - emx*(1. + x))
c
c           Calculate the derivatives with respect to x.
c
            gpx = (-2./(xsq*x)) * (2. - emx*(2. + x*(2. + x)))
            gppx = (-2./(xsq*xsq))
     $      * (-6. + emx*(6. + x*(6. + x*(3. + x))))
c
c           Calculate the derivatives with respect to ionic strength.
c
            dxdi = alphai/(2.*sqrt(fxi))
            dgpit(1,i,nap) = dxdi * gpx
            dgpit(2,i,nap) = dxdi * (gppx*dxdi - 0.5*gpx/fxi)
          endif
        enddo
      enddo
c
      end


      double complex function C0fb2m(u,msq)
C     C0(Pc,Pg,0,msq,msq)=
C     C0(msq,0,u,0,msq,msq) (LT notation)
C  C0fb2m(u,msq)=(Pi^2/6-Li2[u/m2])/(u-msq) with u=(Pc+Pg)^2;
      implicit none
      include 'constants.f'
      double precision u,msq,ubar,r,omr,ddilog
      double complex lnrat,wlogu,dilogu
      ubar=u-msq
      r=-ubar/msq
      omr=1d0-r
      if (omr .gt. one) then
         wlogu=lnrat(-ubar,msq)
         dilogu=dcmplx(pisqo6-ddilog(r))-wlogu*dcmplx(log(omr))
      else
         dilogu=dcmplx(ddilog(omr))
      endif
      C0fb2m=(dcmplx(pisqo6)-dilogu)/ubar
      return
      end


!*==cgetsls.f90  processed by SPAG 7.51RB at 20:08 on  3 Mar 2022
!> \brief \b CGETSLS
!
!  Definition:
!  ===========
!
!       SUBROUTINE CGETSLS( TRANS, M, N, NRHS, A, LDA, B, LDB,
!     $                     WORK, LWORK, INFO )
!
!       .. Scalar Arguments ..
!       CHARACTER          TRANS
!       INTEGER            INFO, LDA, LDB, LWORK, M, N, NRHS
!       ..
!       .. Array Arguments ..
!       COMPLEX            A( LDA, * ), B( LDB, * ), WORK( * )
!       ..
!
!
!> \par Purpose:
!  =============
!>
!> \verbatim
!>
!> CGETSLS solves overdetermined or underdetermined complex linear systems
!> involving an M-by-N matrix A, using a tall skinny QR or short wide LQ
!> factorization of A.  It is assumed that A has full rank.
!>
!>
!>
!> The following options are provided:
!>
!> 1. If TRANS = 'N' and m >= n:  find the least squares solution of
!>    an overdetermined system, i.e., solve the least squares problem
!>                 minimize || B - A*X ||.
!>
!> 2. If TRANS = 'N' and m < n:  find the minimum norm solution of
!>    an underdetermined system A * X = B.
!>
!> 3. If TRANS = 'C' and m >= n:  find the minimum norm solution of
!>    an undetermined system A**T * X = B.
!>
!> 4. If TRANS = 'C' and m < n:  find the least squares solution of
!>    an overdetermined system, i.e., solve the least squares problem
!>                 minimize || B - A**T * X ||.
!>
!> Several right hand side vectors b and solution vectors x can be
!> handled in a single call; they are stored as the columns of the
!> M-by-NRHS right hand side matrix B and the N-by-NRHS solution
!> matrix X.
!> \endverbatim
!
!  Arguments:
!  ==========
!
!> \param[in] TRANS
!> \verbatim
!>          TRANS is CHARACTER*1
!>          = 'N': the linear system involves A;
!>          = 'C': the linear system involves A**H.
!> \endverbatim
!>
!> \param[in] M
!> \verbatim
!>          M is INTEGER
!>          The number of rows of the matrix A.  M >= 0.
!> \endverbatim
!>
!> \param[in] N
!> \verbatim
!>          N is INTEGER
!>          The number of columns 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 matrices B and X. NRHS >=0.
!> \endverbatim
!>
!> \param[in,out] A
!> \verbatim
!>          A is COMPLEX array, dimension (LDA,N)
!>          On entry, the M-by-N matrix A.
!>          On exit,
!>          A is overwritten by details of its QR or LQ
!>          factorization as returned by CGEQR or CGELQ.
!> \endverbatim
!>
!> \param[in] LDA
!> \verbatim
!>          LDA is INTEGER
!>          The leading dimension of the array A.  LDA >= max(1,M).
!> \endverbatim
!>
!> \param[in,out] B
!> \verbatim
!>          B is COMPLEX array, dimension (LDB,NRHS)
!>          On entry, the matrix B of right hand side vectors, stored
!>          columnwise; B is M-by-NRHS if TRANS = 'N', or N-by-NRHS
!>          if TRANS = 'C'.
!>          On exit, if INFO = 0, B is overwritten by the solution
!>          vectors, stored columnwise:
!>          if TRANS = 'N' and m >= n, rows 1 to n of B contain the least
!>          squares solution vectors.
!>          if TRANS = 'N' and m < n, rows 1 to N of B contain the
!>          minimum norm solution vectors;
!>          if TRANS = 'C' and m >= n, rows 1 to M of B contain the
!>          minimum norm solution vectors;
!>          if TRANS = 'C' and m < n, rows 1 to M of B contain the
!>          least squares solution vectors.
!> \endverbatim
!>
!> \param[in] LDB
!> \verbatim
!>          LDB is INTEGER
!>          The leading dimension of the array B. LDB >= MAX(1,M,N).
!> \endverbatim
!>
!> \param[out] WORK
!> \verbatim
!>          (workspace) COMPLEX array, dimension (MAX(1,LWORK))
!>          On exit, if INFO = 0, WORK(1) contains optimal (or either minimal
!>          or optimal, if query was assumed) LWORK.
!>          See LWORK for details.
!> \endverbatim
!>
!> \param[in] LWORK
!> \verbatim
!>          LWORK is INTEGER
!>          The dimension of the array WORK.
!>          If LWORK = -1 or -2, then a workspace query is assumed.
!>          If LWORK = -1, the routine calculates optimal size of WORK for the
!>          optimal performance and returns this value in WORK(1).
!>          If LWORK = -2, the routine calculates minimal size of WORK and
!>          returns this value in WORK(1).
!> \endverbatim
!>
!> \param[out] INFO
!> \verbatim
!>          INFO is INTEGER
!>          = 0:  successful exit
!>          < 0:  if INFO = -i, the i-th argument had an illegal value
!>          > 0:  if INFO =  i, the i-th diagonal element of the
!>                triangular factor of A is zero, so that A does not have
!>                full rank; the least squares solution could not be
!>                computed.
!> \endverbatim
!
!  Authors:
!  ========
!
!> \author Univ. of Tennessee
!> \author Univ. of California Berkeley
!> \author Univ. of Colorado Denver
!> \author NAG Ltd.
!
!> \date June 2017
!
!> \ingroup complexGEsolve
!
!  =====================================================================
      SUBROUTINE CGETSLS(Trans,M,N,Nrhs,A,Lda,B,Ldb,Work,Lwork,Info)
      IMPLICIT NONE
!*--CGETSLS165
!
!  -- LAPACK driver routine (version 3.7.1) --
!  -- LAPACK is a software package provided by Univ. of Tennessee,    --
!  -- Univ. of California Berkeley, Univ. of Colorado Denver and NAG Ltd..--
!     June 2017
!
!     .. Scalar Arguments ..
      CHARACTER Trans
      INTEGER Info , Lda , Ldb , Lwork , M , N , Nrhs
!     ..
!     .. Array Arguments ..
      COMPLEX A(Lda,*) , B(Ldb,*) , Work(*)
!
!     ..
!
!  =====================================================================
!
!     .. Parameters ..
      REAL ZERO , ONE
      PARAMETER (ZERO=0.0E0,ONE=1.0E0)
      COMPLEX CZERO
      PARAMETER (CZERO=(0.0E+0,0.0E+0))
!     ..
!     .. Local Scalars ..
      LOGICAL lquery , tran
      INTEGER i , iascl , ibscl , j , minmn , maxmn , brow , scllen ,   &
     &        mnk , tszo , tszm , lwo , lwm , lw1 , lw2 , wsizeo ,      &
     &        wsizem , info2
      REAL anrm , bignum , bnrm , smlnum , dum(1)
      COMPLEX tq(5) , workq(1)
!     ..
!     .. External Functions ..
      LOGICAL LSAME
      INTEGER ILAENV
      REAL SLAMCH , CLANGE
      EXTERNAL LSAME , ILAENV , SLABAD , SLAMCH , CLANGE
!     ..
!     .. External Subroutines ..
      EXTERNAL CGEQR , CGEMQR , CLASCL , CLASET , CTRTRS , XERBLA ,     &
     &         CGELQ , CGEMLQ
!     ..
!     .. Intrinsic Functions ..
      INTRINSIC REAL , MAX , MIN , INT
!     ..
!     .. Executable Statements ..
!
!     Test the input arguments.
!
      Info = 0
      minmn = MIN(M,N)
      maxmn = MAX(M,N)
      mnk = MAX(minmn,Nrhs)
      tran = LSAME(Trans,'C')
!
      lquery = (Lwork==-1 .OR. Lwork==-2)
      IF ( .NOT.(LSAME(Trans,'N') .OR. LSAME(Trans,'C')) ) THEN
         Info = -1
      ELSEIF ( M<0 ) THEN
         Info = -2
      ELSEIF ( N<0 ) THEN
         Info = -3
      ELSEIF ( Nrhs<0 ) THEN
         Info = -4
      ELSEIF ( Lda<MAX(1,M) ) THEN
         Info = -6
      ELSEIF ( Ldb<MAX(1,M,N) ) THEN
         Info = -8
      ENDIF
!
      IF ( Info==0 ) THEN
!
!     Determine the block size and minimum LWORK
!
         IF ( M>=N ) THEN
            CALL CGEQR(M,N,A,Lda,tq,-1,workq,-1,info2)
            tszo = INT(tq(1))
            lwo = INT(workq(1))
            CALL CGEMQR('L',Trans,M,Nrhs,N,A,Lda,tq,tszo,B,Ldb,workq,-1,&
     &                  info2)
            lwo = MAX(lwo,INT(workq(1)))
            CALL CGEQR(M,N,A,Lda,tq,-2,workq,-2,info2)
            tszm = INT(tq(1))
            lwm = INT(workq(1))
            CALL CGEMQR('L',Trans,M,Nrhs,N,A,Lda,tq,tszm,B,Ldb,workq,-1,&
     &                  info2)
            lwm = MAX(lwm,INT(workq(1)))
            wsizeo = tszo + lwo
            wsizem = tszm + lwm
         ELSE
            CALL CGELQ(M,N,A,Lda,tq,-1,workq,-1,info2)
            tszo = INT(tq(1))
            lwo = INT(workq(1))
            CALL CGEMLQ('L',Trans,N,Nrhs,M,A,Lda,tq,tszo,B,Ldb,workq,-1,&
     &                  info2)
            lwo = MAX(lwo,INT(workq(1)))
            CALL CGELQ(M,N,A,Lda,tq,-2,workq,-2,info2)
            tszm = INT(tq(1))
            lwm = INT(workq(1))
            CALL CGEMLQ('L',Trans,N,Nrhs,M,A,Lda,tq,tszm,B,Ldb,workq,-1,&
     &                  info2)
            lwm = MAX(lwm,INT(workq(1)))
            wsizeo = tszo + lwo
            wsizem = tszm + lwm
         ENDIF
!
         IF ( (Lwork<wsizem) .AND. (.NOT.lquery) ) Info = -10
!
      ENDIF
!
      IF ( Info/=0 ) THEN
         CALL XERBLA('CGETSLS',-Info)
         Work(1) = REAL(wsizeo)
         RETURN
      ENDIF
      IF ( lquery ) THEN
         IF ( Lwork==-1 ) Work(1) = REAL(wsizeo)
         IF ( Lwork==-2 ) Work(1) = REAL(wsizem)
         RETURN
      ENDIF
      IF ( Lwork<wsizeo ) THEN
         lw1 = tszm
         lw2 = lwm
      ELSE
         lw1 = tszo
         lw2 = lwo
      ENDIF
!
!     Quick return if possible
!
      IF ( MIN(M,N,Nrhs)==0 ) THEN
         CALL CLASET('FULL',MAX(M,N),Nrhs,CZERO,CZERO,B,Ldb)
         RETURN
      ENDIF
!
!     Get machine parameters
!
      smlnum = SLAMCH('S')/SLAMCH('P')
      bignum = ONE/smlnum
      CALL SLABAD(smlnum,bignum)
!
!     Scale A, B if max element outside range [SMLNUM,BIGNUM]
!
      anrm = CLANGE('M',M,N,A,Lda,dum)
      iascl = 0
      IF ( anrm>ZERO .AND. anrm<smlnum ) THEN
!
!        Scale matrix norm up to SMLNUM
!
         CALL CLASCL('G',0,0,anrm,smlnum,M,N,A,Lda,Info)
         iascl = 1
      ELSEIF ( anrm>bignum ) THEN
!
!        Scale matrix norm down to BIGNUM
!
         CALL CLASCL('G',0,0,anrm,bignum,M,N,A,Lda,Info)
         iascl = 2
      ELSEIF ( anrm==ZERO ) THEN
!
!        Matrix all zero. Return zero solution.
!
         CALL CLASET('F',maxmn,Nrhs,CZERO,CZERO,B,Ldb)
         GOTO 100
      ENDIF
!
      brow = M
      IF ( tran ) brow = N
      bnrm = CLANGE('M',brow,Nrhs,B,Ldb,dum)
      ibscl = 0
      IF ( bnrm>ZERO .AND. bnrm<smlnum ) THEN
!
!        Scale matrix norm up to SMLNUM
!
         CALL CLASCL('G',0,0,bnrm,smlnum,brow,Nrhs,B,Ldb,Info)
         ibscl = 1
      ELSEIF ( bnrm>bignum ) THEN
!
!        Scale matrix norm down to BIGNUM
!
         CALL CLASCL('G',0,0,bnrm,bignum,brow,Nrhs,B,Ldb,Info)
         ibscl = 2
      ENDIF
!
      IF ( M>=N ) THEN
!
!        compute QR factorization of A
!
         CALL CGEQR(M,N,A,Lda,Work(lw2+1),lw1,Work(1),lw2,Info)
         IF ( .NOT.tran ) THEN
!
!           Least-Squares Problem min || A * X - B ||
!
!           B(1:M,1:NRHS) := Q**T * B(1:M,1:NRHS)
!
            CALL CGEMQR('L','C',M,Nrhs,N,A,Lda,Work(lw2+1),lw1,B,Ldb,   &
     &                  Work(1),lw2,Info)
!
!           B(1:N,1:NRHS) := inv(R) * B(1:N,1:NRHS)
!
            CALL CTRTRS('U','N','N',N,Nrhs,A,Lda,B,Ldb,Info)
            IF ( Info>0 ) RETURN
            scllen = N
         ELSE
!
!           Overdetermined system of equations A**T * X = B
!
!           B(1:N,1:NRHS) := inv(R**T) * B(1:N,1:NRHS)
!
            CALL CTRTRS('U','C','N',N,Nrhs,A,Lda,B,Ldb,Info)
!
            IF ( Info>0 ) RETURN
!
!           B(N+1:M,1:NRHS) = CZERO
!
            DO j = 1 , Nrhs
               DO i = N + 1 , M
                  B(i,j) = CZERO
               ENDDO
            ENDDO
!
!           B(1:M,1:NRHS) := Q(1:N,:) * B(1:N,1:NRHS)
!
            CALL CGEMQR('L','N',M,Nrhs,N,A,Lda,Work(lw2+1),lw1,B,Ldb,   &
     &                  Work(1),lw2,Info)
!
            scllen = M
!
         ENDIF
!
      ELSE
!
!        Compute LQ factorization of A
!
         CALL CGELQ(M,N,A,Lda,Work(lw2+1),lw1,Work(1),lw2,Info)
!
!        workspace at least M, optimally M*NB.
!
         IF ( .NOT.tran ) THEN
!
!           underdetermined system of equations A * X = B
!
!           B(1:M,1:NRHS) := inv(L) * B(1:M,1:NRHS)
!
            CALL CTRTRS('L','N','N',M,Nrhs,A,Lda,B,Ldb,Info)
!
            IF ( Info>0 ) RETURN
!
!           B(M+1:N,1:NRHS) = 0
!
            DO j = 1 , Nrhs
               DO i = M + 1 , N
                  B(i,j) = CZERO
               ENDDO
            ENDDO
!
!           B(1:N,1:NRHS) := Q(1:N,:)**T * B(1:M,1:NRHS)
!
            CALL CGEMLQ('L','C',N,Nrhs,M,A,Lda,Work(lw2+1),lw1,B,Ldb,   &
     &                  Work(1),lw2,Info)
!
!           workspace at least NRHS, optimally NRHS*NB
!
            scllen = N
!
         ELSE
!
!           overdetermined system min || A**T * X - B ||
!
!           B(1:N,1:NRHS) := Q * B(1:N,1:NRHS)
!
            CALL CGEMLQ('L','N',N,Nrhs,M,A,Lda,Work(lw2+1),lw1,B,Ldb,   &
     &                  Work(1),lw2,Info)
!
!           workspace at least NRHS, optimally NRHS*NB
!
!           B(1:M,1:NRHS) := inv(L**T) * B(1:M,1:NRHS)
!
            CALL CTRTRS('L','C','N',M,Nrhs,A,Lda,B,Ldb,Info)
!
            IF ( Info>0 ) RETURN
!
            scllen = M
!
         ENDIF
!
      ENDIF
!
!     Undo scaling
!
      IF ( iascl==1 ) THEN
         CALL CLASCL('G',0,0,anrm,smlnum,scllen,Nrhs,B,Ldb,Info)
      ELSEIF ( iascl==2 ) THEN
         CALL CLASCL('G',0,0,anrm,bignum,scllen,Nrhs,B,Ldb,Info)
      ENDIF
      IF ( ibscl==1 ) THEN
         CALL CLASCL('G',0,0,smlnum,bnrm,scllen,Nrhs,B,Ldb,Info)
      ELSEIF ( ibscl==2 ) THEN
         CALL CLASCL('G',0,0,bignum,bnrm,scllen,Nrhs,B,Ldb,Info)
      ENDIF
!
 100  Work(1) = REAL(tszo+lwo)
!
!     End of ZGETSLS
!
      END SUBROUTINE CGETSLS


      SUBROUTINE SRAND(ISEED)
c from https://www.cisl.ucar.edu/zine/96/spring/articles/3.random-6.html
C
C  This subroutine sets the integer seed to be used with the
C  companion RAND function to the value of ISEED.  A flag is
C  set to indicate that the sequence of pseudo-random numbers
C  for the specified seed should start from the beginning.
C
      COMMON /SEED/JSEED,IFRST
C
      JSEED = ISEED
      IFRST = 0
C
      RETURN
      END
      REAL FUNCTION RAND()
C
C  This function returns a pseudo-random number for each invocation.
C  It is a FORTRAN 77 adaptation of the "Integer Version 2" minimal
C  standard number generator whose Pascal code appears in the article:
C
C     Park, Steven K. and Miller, Keith W., "Random Number Generators:
C     Good Ones are Hard to Find", Communications of the ACM,
C     October, 1988.
C
      PARAMETER (MPLIER=16807,MODLUS=2147483647,MOBYMP=127773,
     +           MOMDMP=2836)
C
      COMMON  /SEED/JSEED,IFRST
      INTEGER HVLUE, LVLUE, TESTV, NEXTN
      SAVE    NEXTN
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
      RAND = REAL(NEXTN)/REAL(MODLUS)
C
      RETURN
      END
      BLOCKDATA RANDBD
      COMMON /SEED/JSEED,IFRST
C
      DATA JSEED,IFRST/123456789,0/
C
      END
      FUNCTION ZBQLNOR(MU,SIGMA)
c From http://www.ucl.ac.uk/~ucakarc/work/software/randgen.f
*
*       Returns a random number Normally distributed with mean
*       MU and standard deviation |SIGMA|, using the Box-Muller
*       algorithm
*
      DOUBLE PRECISION THETA,R,ZBQLNOR,ZBQLU01,PI,MU,SIGMA
      DOUBLE PRECISION SPARE
      INTEGER STATUS
      SAVE STATUS,SPARE,PI
      DATA STATUS /-1/

      IF (STATUS.EQ.-1) PI = 4.0D0*DATAN(1.0D0)

      IF (STATUS.LE.0) THEN
c       THETA = 2.0D0*PI*ZBQLU01(0.0D0)
       THETA = 2.0D0*PI*dble(rand())
c       R = DSQRT( -2.0D0*DLOG(ZBQLU01(0.0D0)) )
       R=DSQRT (-2.0d+0*dlog(dble(rand()) ) )
       ZBQLNOR = (R*DCOS(THETA))
       SPARE = (R*DSIN(THETA))
       STATUS = 1
      ELSE
       ZBQLNOR = SPARE
       STATUS = 0
      ENDIF

      ZBQLNOR = MU + (SIGMA*ZBQLNOR)

      END


      subroutine banslv ( w, nroww, nrow, nbandl, nbandu, b )
c  from  * a practical guide to splines *  by c. de boor
c  companion routine to  banfac . it returns the solution  x  of the
c  linear system  a*x = b  in place of	b , given the lu-factorization
c  for	a  in the workarray  w .
c
c******  i n p u t  ******
c  w, nroww,nrow,nbandl,nbandu.....describe the lu-factorization of a
c	 banded matrix	a  of roder  nrow  as constructed in  banfac .
c	 for details, see  banfac .
c  b.....right side of the system to be solved .
c
c******  o u t p u t  ******
c  b.....contains the solution	x , of order  nrow .
c
c******  m e t h o d  ******
c     (with  a = l*u, as stored in  w,) the unit lower triangular system
c  l(u*x) = b  is solved for  y = u*x, and  y  stored in  b . then the
c  upper triangular system  u*x = y  is solved for  x  . the calcul-
c  ations are so arranged that the innermost loops stay within columns.
c
      integer nbandl,nbandu,nrow,nroww,   i,j,jmax,middle,nrowm1
      real w(nroww,nrow),b(nrow)
      middle = nbandu + 1
      if (nrow .eq. 1)			go to 49
      nrowm1 = nrow - 1
      if (nbandl .eq. 0)		go to 30
c				  forward pass
c	     for i=1,2,...,nrow-1, subtract  right side(i)*(i-th column
c	     of  l )  from right side  (below i-th row) .
      do 21 i=1,nrowm1
	 jmax = min0(nbandl, nrow-i)
	 do 21 j=1,jmax
   21	    b(i+j) = b(i+j) - b(i)*w(middle+j,i)
c				  backward pass
c	     for i=nrow,nrow-1,...,1, divide right side(i) by i-th diag-
c	     onal entry of  u, then subtract  right side(i)*(i-th column
c	     of  u)  from right side  (above i-th row).
   30 if (nbandu .gt. 0)		go to 40
c				 a  is lower triangular .
      do 31 i=1,nrow
   31	 b(i) = b(i)/w(1,i)
					return
   40 i = nrow
   41	 b(i) = b(i)/w(middle,i)
	 jmax = min0(nbandu,i-1)
	 do 45 j=1,jmax
   45	    b(i-j) = b(i-j) - b(i)*w(middle-j,i)
	 i = i - 1
	 if (i .gt. 1)			go to 41
   49 b(1) = b(1)/w(middle,1)
					return
      end


      SUBROUTINE FB01VD( N, M, L, P, LDP, A, LDA, B, LDB, C, LDC, Q,
     $                   LDQ, R, LDR, K, LDK, TOL, IWORK, DWORK, LDWORK,
     $                   INFO )
C
C     SLICOT RELEASE 5.5.
C
C     Copyright (c) 2002-2012 NICONET e.V.
C
C     PURPOSE
C
C     To compute one recursion of the conventional Kalman filter
C     equations. This is one update of the Riccati difference equation
C     and the Kalman filter gain.
C
C     ARGUMENTS
C
C     Input/Output Parameters
C
C     N       (input) INTEGER
C             The actual state dimension, i.e., the order of the
C             matrices P      and A .  N >= 0.
C                       i|i-1      i
C
C     M       (input) INTEGER
C             The actual input dimension, i.e., the order of the matrix
C             Q .  M >= 0.
C              i
C
C     L       (input) INTEGER
C             The actual output dimension, i.e., the order of the matrix
C             R .  L >= 0.
C              i
C
C     P       (input/output) DOUBLE PRECISION array, dimension (LDP,N)
C             On entry, the leading N-by-N part of this array must
C             contain P     , the state covariance matrix at instant
C                      i|i-1
C             (i-1). The upper triangular part only is needed.
C             On exit, if INFO = 0, the leading N-by-N part of this
C             array contains P     , the state covariance matrix at
C                             i+1|i
C             instant i. The strictly lower triangular part is not set.
C             Otherwise, the leading N-by-N part of this array contains
C             P     , its input value.
C              i|i-1
C
C     LDP     INTEGER
C             The leading dimension of array P.  LDP >= MAX(1,N).
C
C     A       (input) DOUBLE PRECISION array, dimension (LDA,N)
C             The leading N-by-N part of this array must contain A ,
C                                                                 i
C             the state transition matrix of the discrete system at
C             instant i.
C
C     LDA     INTEGER
C             The leading dimension of array A.  LDA >= MAX(1,N).
C
C     B       (input) DOUBLE PRECISION array, dimension (LDB,M)
C             The leading N-by-M part of this array must contain B ,
C                                                                 i
C             the input weight matrix of the discrete system at
C             instant i.
C
C     LDB     INTEGER
C             The leading dimension of array B.  LDB >= MAX(1,N).
C
C     C       (input) DOUBLE PRECISION array, dimension (LDC,N)
C             The leading L-by-N part of this array must contain C ,
C                                                                 i
C             the output weight matrix of the discrete system at
C             instant i.
C
C     LDC     INTEGER
C             The leading dimension of array C.  LDC >= MAX(1,L).
C
C     Q       (input) DOUBLE PRECISION array, dimension (LDQ,M)
C             The leading M-by-M part of this array must contain Q ,
C                                                                 i
C             the input (process) noise covariance matrix at instant i.
C             The diagonal elements of this array are modified by the
C             routine, but are restored on exit.
C
C     LDQ     INTEGER
C             The leading dimension of array Q.  LDQ >= MAX(1,M).
C
C     R       (input/output) DOUBLE PRECISION array, dimension (LDR,L)
C             On entry, the leading L-by-L part of this array must
C             contain R , the output (measurement) noise covariance
C                      i
C             matrix at instant i.
C             On exit, if INFO = 0, or INFO = L+1, the leading L-by-L
C                                                                  1/2
C             upper triangular part of this array contains (RINOV )   ,
C                                                                i
C             the square root (left Cholesky factor) of the covariance
C             matrix of the innovations at instant i.
C
C     LDR     INTEGER
C             The leading dimension of array R.  LDR >= MAX(1,L).
C
C     K       (output) DOUBLE PRECISION array, dimension (LDK,L)
C             If INFO = 0, the leading N-by-L part of this array
C             contains K , the Kalman filter gain matrix at instant i.
C                       i
C             If INFO > 0, the leading N-by-L part of this array
C             contains the matrix product P     C'.
C                                          i|i-1 i
C
C     LDK     INTEGER
C             The leading dimension of array K.  LDK >= MAX(1,N).
C
C     Tolerances
C
C     TOL     DOUBLE PRECISION
C             The tolerance to be used to test for near singularity of
C             the matrix RINOV . If the user sets TOL > 0, then the
C                             i
C             given value of TOL is used as a lower bound for the
C             reciprocal condition number of that matrix; a matrix whose
C             estimated condition number is less than 1/TOL is
C             considered to be nonsingular. If the user sets TOL <= 0,
C             then an implicitly computed, default tolerance, defined by
C             TOLDEF = L*L*EPS, is used instead, where EPS is the
C             machine precision (see LAPACK Library routine DLAMCH).
C
C     Workspace
C
C     IWORK   INTEGER array, dimension (L)
C
C     DWORK   DOUBLE PRECISION array, dimension (LDWORK)
C             On exit, if INFO = 0, or INFO = L+1, DWORK(1) returns an
C             estimate of the reciprocal of the condition number (in the
C             1-norm) of the matrix RINOV .
C                                        i
C
C     LDWORK  The length of the array DWORK.
C             LDWORK >= MAX(1,L*N+3*L,N*N,N*M).
C
C     Error Indicator
C
C     INFO    INTEGER
C             = 0:  successful exit;
C             < 0:  if INFO = -k, the k-th argument had an illegal
C                   value;
C             = k:  if INFO = k, 1 <= k <= L, the leading minor of order
C                   k of the matrix RINOV  is not positive-definite, and
C                                        i
C                   its Cholesky factorization could not be completed;
C             = L+1: the matrix RINOV  is singular, i.e., the condition
C                                    i
C                   number estimate of RINOV  (in the 1-norm) exceeds
C                                           i
C                   1/TOL.
C
C     METHOD
C
C     The conventional Kalman filter gain used at the i-th recursion
C     step is of the form
C
C                            -1
C        K  = P     C'  RINOV  ,
C         i    i|i-1 i       i
C
C     where RINOV  = C P     C' + R , and the state covariance matrix
C                i    i i|i-1 i    i
C
C     P      is updated by the discrete-time difference Riccati equation
C      i|i-1
C
C        P      = A  (P      - K C P     ) A'  + B Q B'.
C         i+1|i    i   i|i-1    i i i|i-1   i     i i i
C
C     Using these two updates, the combined time and measurement update
C     of the state X      is given by
C                   i|i-1
C
C        X      = A X      + A K (Y  - C X     ),
C         i+1|i    i i|i-1    i i  i    i i|i-1
C
C     where Y  is the new observation at step i.
C            i
C
C     REFERENCES
C
C     [1] Anderson, B.D.O. and Moore, J.B.
C         Optimal Filtering,
C         Prentice Hall, Englewood Cliffs, New Jersey, 1979.
C
C     [2] Verhaegen, M.H.G. and Van Dooren, P.
C         Numerical Aspects of Different Kalman Filter Implementations.
C         IEEE Trans. Auto. Contr., AC-31, pp. 907-917, 1986.
C
C     NUMERICAL ASPECTS
C
C     The algorithm requires approximately
C
C             3   2
C      3/2 x N + N  x (3 x L + M/2)
C
C     operations.
C
C     CONTRIBUTORS
C
C     Release 3.0: V. Sima, Katholieke Univ. Leuven, Belgium, Feb. 1997.
C     Supersedes Release 2.0 routine FB01JD by M.H.G. Verhaegen,
C     M. Vanbegin, and P. Van Dooren.
C
C     REVISIONS
C
C     February 20, 1998, November 20, 2003, April 20, 2004.
C
C     KEYWORDS
C
C     Kalman filtering, optimal filtering, recursive estimation.
C
C     ******************************************************************
C
C     .. Parameters ..
      DOUBLE PRECISION  ZERO, ONE, TWO
      PARAMETER         ( ZERO = 0.0D0, ONE = 1.0D0, TWO = 2.0D0 )
C     .. Scalar Arguments ..
      INTEGER           INFO, L, LDA, LDB, LDC, LDK, LDP, LDQ, LDR,
     $                  LDWORK, M, N
      DOUBLE PRECISION  TOL
C     .. Array Arguments ..
      INTEGER           IWORK(*)
      DOUBLE PRECISION  A(LDA,*), B(LDB,*), C(LDC,*), DWORK(*),
     $                  K(LDK,*), P(LDP,*), Q(LDQ,*), R(LDR,*)
C     .. Local Scalars ..
      INTEGER           J, JWORK, LDW, N1
      DOUBLE PRECISION  RCOND, RNORM, TOLDEF
C     .. External Functions ..
      DOUBLE PRECISION  DLAMCH, DLANSY
      EXTERNAL          DLAMCH, DLANSY
C     .. External Subroutines ..
      EXTERNAL          DAXPY, DCOPY, DGEMV, DLACPY, DLASET, DPOCON,
     $                  DPOTRF, DSCAL, DTRMM, DTRSM, MB01RD, XERBLA
C     .. Intrinsic Functions ..
      INTRINSIC         DBLE, MAX
C     .. Executable Statements ..
C
C     Test the input scalar arguments.
C
      INFO = 0
      N1 = MAX( 1, N )
      IF( N.LT.0 ) THEN
         INFO = -1
      ELSE IF( M.LT.0 ) THEN
         INFO = -2
      ELSE IF( L.LT.0 ) THEN
         INFO = -3
      ELSE IF( LDP.LT.N1 ) THEN
         INFO = -5
      ELSE IF( LDA.LT.N1 ) THEN
         INFO = -7
      ELSE IF( LDB.LT.N1 ) THEN
         INFO = -9
      ELSE IF( LDC.LT.MAX( 1, L ) ) THEN
         INFO = -11
      ELSE IF( LDQ.LT.MAX( 1, M ) ) THEN
         INFO = -13
      ELSE IF( LDR.LT.MAX( 1, L ) ) THEN
         INFO = -15
      ELSE IF( LDK.LT.N1 ) THEN
         INFO = -17
      ELSE IF( LDWORK.LT.MAX( 1, L*N + 3*L, N*N, N*M ) ) THEN
         INFO = -21
      END IF
C
      IF ( INFO.NE.0 ) THEN
C
C        Error return.
C
         CALL XERBLA( 'FB01VD', -INFO )
         RETURN
      END IF
C
C     Quick return if possible.
C
      IF ( MAX( N, L ).EQ.0 ) THEN
         DWORK(1) = ONE
         RETURN
      END IF
C
C     Efficiently compute RINOV = CPC' + R in R and put CP in DWORK and
C     PC' in K. (The content of DWORK on exit from MB01RD is used.)
C     Workspace: need L*N.
C
C     (Note: Comments in the code beginning "Workspace:" describe the
C     minimal amount of real workspace needed at that point in the
C     code.)
C
      CALL MB01RD( 'Upper', 'No transpose', L, N, ONE, ONE, R, LDR, C,
     $             LDC, P, LDP, DWORK, LDWORK, INFO )
      LDW = MAX( 1, L )
C
      DO 10 J = 1, L
         CALL DCOPY( N, DWORK(J), LDW, K(1,J), 1 )
   10 CONTINUE
C
      CALL DLACPY( 'Full', L, N, C, LDC, DWORK, LDW )
      CALL DTRMM( 'Right', 'Upper', 'Transpose', 'Non-unit', L, N, ONE,
     $            P, LDP, DWORK, LDW )
      CALL DSCAL( N, TWO, P, LDP+1 )
C
      DO 20 J = 1, L
         CALL DAXPY( N, ONE, K(1,J), 1, DWORK(J), LDW )
         CALL DCOPY( N, DWORK(J), LDW, K(1,J), 1 )
   20 CONTINUE
C
C     Calculate the Cholesky decomposition U'U of the innovation
C     covariance matrix RINOV, and its reciprocal condition number.
C     Workspace: need L*N + 3*L.
C
      JWORK = L*N + 1
      RNORM = DLANSY( '1-norm', 'Upper', L, R, LDR, DWORK(JWORK) )
C
      TOLDEF = TOL
      IF ( TOLDEF.LE.ZERO )
     $   TOLDEF = DBLE( L*L )*DLAMCH( 'Epsilon' )
      CALL DPOTRF( 'Upper', L, R, LDR, INFO )
      IF ( INFO.NE.0 )
     $   RETURN
C
      CALL DPOCON( 'Upper', L, R, LDR, RNORM, RCOND, DWORK(JWORK),
     $             IWORK, INFO )
C
      IF ( RCOND.LT.TOLDEF ) THEN
C
C        Error return: RINOV is numerically singular.
C
         INFO = L+1
         DWORK(1) = RCOND
         RETURN
      END IF
C
      IF ( L.GT.1 )
     $   CALL DLASET( 'Lower', L-1, L-1, ZERO, ZERO, R(2,1),LDR )
C                                                          -1
C     Calculate the Kalman filter gain matrix  K = PC'RINOV .
C     Workspace: need L*N.
C
      CALL DTRSM( 'Right', 'Upper', 'No transpose', 'Non-unit', N, L,
     $            ONE, R, LDR, K, LDK )
      CALL DTRSM( 'Right', 'Upper', 'Transpose', 'Non-unit', N, L,
     $            ONE, R, LDR, K, LDK )
C
C     First part of the Riccati equation update: compute A(P-KCP)A'.
C     The upper triangular part of the symmetric matrix P-KCP is formed.
C     Workspace: need max(L*N,N*N).
C
      JWORK = 1
C
      DO 30 J = 1, N
         CALL DGEMV( 'No transpose', J, L, -ONE, K, LDK, DWORK(JWORK),
     $               1, ONE, P(1,J), 1 )
         JWORK = JWORK + L
   30 CONTINUE
C
      CALL MB01RD( 'Upper', 'No transpose', N, N, ZERO, ONE, P, LDP, A,
     $             LDA, P, LDP, DWORK, LDWORK, INFO )
C
C     Second part of the Riccati equation update: add BQB'.
C     Workspace: need N*M.
C
      CALL MB01RD( 'Upper', 'No transpose', N, M, ONE, ONE, P, LDP, B,
     $             LDB, Q, LDQ, DWORK, LDWORK, INFO )
      CALL DSCAL( M, TWO, Q, LDQ+1 )
C
C     Set the reciprocal of the condition number estimate.
C
      DWORK(1) = RCOND
C
      RETURN
C *** Last line of FB01VD ***
      END


        SUBROUTINE C4FFT(C,ID1,ID2,N1,N2,N3,N4,W1,W2,W3,W4,ISIG,IORD,
     $                   IWORK,IERR)
*
*PURPOSE:
*       THIS ROUTINE PERFORMS A 4-DIMENSIONAL FOURIER TRANSFORM,
*       OF ORDER N1*N2*N3*N4
****USAGE:
*       THE USER IS EXPECTED TO PROVIDE THE DATA IN A 4-DIMENSIONAL
*       COMPLEX ARRAY C, DIMENSIONED IN THE CALLING PROGRAM
*       DIMENSION C(ID1,ID2,N3,N4)
*       FOR OPTIMAL PERFORMANCE, ID1 AND ID2 SHOULD BE ODD, AND EQUAL
*       RESPECTIVELY TO N1 OR N1+1, N2 OR N2+1, DEPENDING IF N1 AND N2
*       ARE ODD OR EVEN. THE ROUTINE IS INTENDED FOR REPEATED USAGE, THUS
*       SEPARATE SET-UP AND OPERATING CALLS ARE AVAILABLE: THE USER SHOULD
*       ALLWAYS PERFORM A SET-UP CALL ( ISIG=0 ) PASSING THE
*       CORRECT VALUES OF THE ARGUMENTS, BEFORE PERFORMING THE
*       ACTUAL TRANSFORM ( ISIG= +1 OR -1 ); THE USER CAN CHOOSE
*       WHETHER TO OBTAIN THE RESULT OF A DIRECT TRANSFORM IN
*       NATURAL ORDER (ISIG=-1,IORD=1) OR IN "BIT-REVERSED" ORDER
*       (ISIG=-1,IORD=0); THIS CHOICE SAVES SOME COMPUTER TIME
*       IN CASES DISCUSSED IN THE LONG WRITE-UP. ANALOGOUSLY, THE
*       INVERSE TRANSFORM ACCEPTS INPUT IN NATURAL ORDER (ISIG=1,
*       IORD=1) OR IN "BIT-REVERSED" ORDER ( ISIG=1,IORD=-1).
****ARGUMENTS
*       C:
*          DECLARED COMPLEX C(ID1,ID2,N3,N4) IN THE CALLING PROGRAM
*          INPUT: ARRAY TO BE TRANSFORMED;
*          OUTPUT: TRANSFORMED ARRAY IF ISIG.NE.0; UNCHANGED IF ISIG.EQ.0
*       ID1:
*          INTEGER;
*          INPUT: FIRST DIMENSION OF C, AS DECLARED IN THE CALLING PROGRAM
*       ID2:
*          INTEGER;
*          INPUT: SECOND DIMENSION OF C, AS DECLARED IN THE CALLING
*               PROGRAM
*       N1:
*          INTEGER;
*          INPUT: ORDER OF THE TRANSFORM ALONG THE FIRST DIMENSION;
*                N1.LE.ID1;  N1 MUST BE A PRODUCT OF POWERS OF 2,3,5;
*       N2:
*          INTEGER;
*          INPUT: ORDER OF THE TRANSFORM ALONG THE SECOND DIMENSION;
*                 N2.LE.ID2 ; N2 MUST BE A PRODUCT OF POWERS OF 2,3,5;
*       N3:
*          INTEGER;
*          INPUT: THIRD DIMENSION OF C, AS DECLARED IN THE CALLING PROGRAM,
*                 AND ORDER OF THE TRANSFORM ALONG THE THIRD DIMENSION
*                 N3 MUST BE A PRODUCT OF POWERS OF 2,3,5;
*       N4:
*         INTEGER;
*         INPUT: ORDER OF THE TRANSFORM ALONG THE FOURTH DIMENSION
*                 N1 MUST BE A PRODUCT OF POWERS OF 2,3,5;
*       W1,W2,W3,W4:
*         THEY MUST BE DECLARED IN THE CALLING PROGRAM AS
*         COMPLEX W1(2*N1+7),W2(2*N2+7),W3(2*N3+7),W4(2*N4+7)
*         IF ANY OF THE N'S ARE EQUAL, THE CORRESPONDING W'S
*         DON'T NEED TO BE DISTINCT.
*         INPUT: IF ISIG.NE.0, THE W'S MUST CONTAIN THE TABLES PREPARED
*                BY A PREVIOUS CALL WITH THE SAME ARGUMENTS BUT ISIG.EQ.0.
*         OUTPUT: IF ISIG.EQ.0, THE W ARE FILLED WITH THE TABLES REQUIRED
*                 FOR THE TRANSFORM. IF ISIG.NE.0, UNCHANGED.
*       IORD:
*         INTEGER;
*         INPUT: IF IORD.EQ.0, NO REORDERING IS PERFORMED AFTER A DIRECT
*                TRANSFORM, SO THAT THE RESULTS ARE LEFT IN "BIT REVERSED"
*                ORDER; AND NO PERMUTATION IS PERFORMED BEFORE THE INVERSE
*                TRANSFORM, SO THAT THE INPUT MATRIX IS REQUIRED TO BE
*               ALREADY IN "BIT-REVERSED" ORDER; IF IORD.NE.0, OUTPUT OF
*              DIRECT  TRANSFORM AND INPUT TO INVERSE TRANSFORM ARE IN
*            NATURAL ORDER
*       ISIG:
*         INTEGER;
*         INPUT: IF ISIG.LT.0  , DIRECT TRANSFORM ( C(J)EXP(-2*PI*I*J*K/N))
*                IF ISIG.GT.0  ,INVERSE TRANSFORM ( C(J)EXP(2*PI*I*J*K/N))
*                IF ISIG.EQ.0  , SETUP RUN ( FILLING OF THE W'S )
*       IWORK:
*         INTEGER ARRAY, OF SIZE MAX(N1,N2,N3,N4)
*         WORK AREA;
*       IERR:
*         INTEGER:
*         OUTPUT: ERROR CODE:
*                 0 :NO ERRORS;
*                 1 :ID1.LT.N1 OR ID2.LT.N2
*                 2 :ONE OF THE N'S CONTAINS A FACTOR DIFFERENT FROM 2,3,5
*                 3 :IN A CALL WITH ISIG.NE.0, SOME OF THE W'S CONTAINED
*                      ILLEGAL VALUES
*
        COMPLEX C(0:*)
        INTEGER W1(-14:*),W2(-14:*),W3(-14:*),W4(-14:*)
        INTEGER IWORK(*)
        INTEGER IDERR,FACERR,TBERR
        PARAMETER(IDERR=1,FACERR=2,TBERR=3)

        IF(ID1.LT.N1.OR.ID2.LT.N2)THEN
                IERR=IDERR
                RETURN
        ENDIF

        IERR=0
        IF(ISIG.EQ.0)THEN

                CALL MFFTP(N1,W1,0,IERR)
                IF(IERR.NE.0)RETURN
                IF(N2.EQ.N1)THEN
                        CALL MFFTZ0(W1,1,4*N1+14,W2,1)
                ELSE
                        CALL MFFTP(N2,W2,0,IERR)
                        IF(IERR.NE.0)RETURN
                ENDIF
                IF(N3.EQ.N1)THEN
                        CALL MFFTZ0(W1,1,4*N1+14,W3,1)
                ELSE IF(N3.EQ.N2)THEN
                        CALL MFFTZ0(W2,1,4*N2+14,W3,1)
                ELSE
                        CALL MFFTP(N3,W3,0,IERR)
                        IF(IERR.NE.0)RETURN
                ENDIF
                IF(N4.EQ.N1)THEN
                        CALL MFFTZ0(W1,1,4*N1+14,W4,1)
                ELSE IF(N4.EQ.N2)THEN
                        CALL MFFTZ0(W2,1,4*N2+14,W4,1)
                ELSE IF (N4.EQ.N3)THEN
                        CALL MFFTZ0(W3,1,4*N3+14,W4,1)
                ELSE
                        CALL MFFTP(N4,W4,0,IERR)
                        IF(IERR.NE.0)RETURN
                ENDIF
                RETURN

        ELSE  IF(ISIG.GT.0)THEN

                IF(IORD.NE.0)THEN
                        CALL MFFTOV(C,1,ID1,N1,ID2*N3*N4,W1(3*N1),IWORK)
                        CALL MFFTOM(C,ID1,1,ID1*ID2,N2,N1,N3*N4,W2(3*N2)
     $                             ,IWORK)
                        CALL MFFTOM(C,ID1*ID2,ID1*ID2*N3,1,N3,N4,ID1*N2,
     $                              W3(3*N3),IWORK)
                        CALL MFFTOV(C,ID1*ID2*N3,1,N4,ID1*ID2*N3,
     $                              W4(3*N4),IWORK)
                ENDIF

                CALL MFFTIV(C,1,ID1,N1,ID2*N3*N4,W1,IERR)
                IF(IERR.NE.0)RETURN
                CALL MFFTIM(C,ID1,1,ID1*ID2,N2,N1,N3*N4,W2,IERR)
                IF(IERR.NE.0)RETURN
                CALL MFFTIM(C,ID1*ID2,ID1*ID2*N3,1,N3,N4,ID1*N2,W3,IERR)
                IF(IERR.NE.0)RETURN
                CALL MFFTIV(C,ID1*ID2*N3,1,N4,ID1*ID2*N3,W4,IERR)

        ELSE

                CALL MFFTDV(C,1,ID1,N1,ID2*N3*N4,W1,IERR)
                IF(IERR.NE.0)RETURN
                CALL MFFTDM(C,ID1,1,ID1*ID2,N2,N1,N3*N4,W2,IERR)
                IF(IERR.NE.0)RETURN
                CALL MFFTDM(C,ID1*ID2,ID1*ID2*N3,1,N3,N4,ID1*N2,W3,IERR)
                IF(IERR.NE.0)RETURN
                CALL MFFTDV(C,ID1*ID2*N3,1,N4,ID1*ID2*N3,W4,IERR)

                IF(IORD.NE.0)THEN
                        CALL MFFTOV(C,1,ID1,N1,ID2*N3*N4,W1(2*N1),IWORK)
                        CALL MFFTOM(C,ID1,1,ID1*ID2,N2,N1,N3*N4,W2(2*N2)
     $                             ,IWORK)
                        CALL MFFTOM(C,ID1*ID2,ID1*ID2*N3,1,N3,N4,ID1*N2,
     $                              W3(2*N3),IWORK)
                        CALL MFFTOV(C,ID1*ID2*N3,1,N4,ID1*ID2*N3,
     $                              W4(2*N4),IWORK)
                ENDIF
        ENDIF
        END


!  *********************************************************************
!  *                                                                   *
!  *                           function dcvab3                         *
!  *                                                                   *
!  *********************************************************************
!  Double Precision Version 1.5
!  Written by Gordon A. Fenton, TUNS, July 17, 1992
!  Latest Update: Jul 2, 2003
!
!  PURPOSE  returns the covariance between two 3-D local averages, one
!           having eight times the volume of the other. Used by LAS3G.
!
!  This function evaluates the covariance between two local averages in
!  3-dimensional space. One local average is assumed to be of size Dx x Dy x Dz
!  and the other of size 2*Dx x 2*Dy x 2*Dz and they are separated in space by
!  a central distance having components Tx = C1*Dx, Ty = C2*Dy, and
!  Tz = C3*Dz.
!
!  The covariance is obtained by a 16-pt Gaussian quadrature of a
!  6-D integral collapsed (by assuming the covariance function to be
!  quadrant symmetric) to a 3-D integral.
!
!  NOTE: if the covariance function is not quadrant symmetric, this
!        routine will return erroneous results. Quadrant symmetry means
!        that cov(X,Y,Z) = cov(-X,Y,Z) = cov(X,-Y,Z), etc, where
!        cov(X,Y,Z) is a function returning the covariance between
!        points separated by lag (X,Y,Z), as discussed next.
!
!  The variance/covariance function is referenced herein using a call of the
!  form
!
!          V = cov( X, Y, Z )
!
!  where X, Y, and Z are the lag distances between the points in the
!  field.
!  Parameters, such as var, pb, dthx, dthy, and dthz are passed to the
!  variance/covariance function and this routine via the common block
!  'dparam'.
!
!  Arguments to this function are as follows
!
!    cov       external function provided by the user. On each invocation,
!              this routine calls cov up to 27*(5^3) = 3375 times.
!
!    Dx        x-dimension of the smaller local average cell. (input)
!
!    Dy        y-dimension of the smaller local average cell. (input)
!
!    Dz        z-dimension of the smaller local average cell. (input)
!
!    C1        x-direction distance between local average centers is C1*Dx.
!              (input)
!
!    C2        y-direction distance between local average centers is C2*Dy.
!              (input)
!
!    C3        z-direction distance between local average centers is C3*Dz.
!              (input)
!
!  REVISION HISTORY:
!  1.1	now including a Gaussian Quadrature integration option as an
!	alternative to the variance function approach to evaluate
!	covariances between local averages. (Jun 16/00)
!  1.2	use Gauss quadrature unless intervals overlap, eliminated lvarfn
!	(Apr 5/01)
!  1.3	now use Gauss quadrature for everything. (May 8/01)
!  1.4	now use 16 point Gauss quadrature (Mar 20/03)
!  1.5	simplification below only correct for volume completely enclosed.
!	Now using 20-point Gauss quadrature (Jul 2/03)
!---------------------------------------------------------------------------
      real*8 function dcvab3( cov, Dx, Dy, Dz, C1, C2, C3 )
      parameter (NG = 20)
      implicit real*8 (a-h,o-z)
      dimension w(NG), z(NG)

      external cov
      common/dparam/ var, pb, dthx, dthy, dthz

      data zero/0.d0/, half/0.5d0/, one/1.d0/, onept5/1.5d0/, two/2.d0/
      data eighth/0.125d0/, fv12th/0.001953125d0/	!(= 1/512)
      data halfe/0.500000000001d0/			! half plus a wee bit
      data w/0.017614007139152118312d0, 0.040601429800386941331d0,        &
             0.062672048334109063570d0, 0.083276741576704748725d0,        &
             0.101930119817240435037d0, 0.118194531961518417312d0,        &
             0.131688638449176626898d0, 0.142096109318382051329d0,        &
             0.149172986472603746788d0, 0.152753387130725850698d0,        &
             0.152753387130725850698d0, 0.149172986472603746788d0,        &
             0.142096109318382051329d0, 0.131688638449176626898d0,        &
             0.118194531961518417312d0, 0.101930119817240435037d0,        &
             0.083276741576704748725d0, 0.062672048334109063570d0,        &
             0.040601429800386941331d0, 0.017614007139152118312d0/
      data z/-.993128599185094924786d0, -.963971927277913791268d0,        &
             -.912234428251325905868d0, -.839116971822218823395d0,        &
             -.746331906460150792614d0, -.636053680726515025453d0,        &
             -.510867001950827098004d0, -.373706088715419560673d0,        &
             -.227785851141645078080d0, -.076526521133497333755d0,        &
             0.076526521133497333755d0, 0.227785851141645078080d0,        &
             0.373706088715419560673d0, 0.510867001950827098004d0,        &
             0.636053680726515025453d0, 0.746331906460150792614d0,        &
             0.839116971822218823395d0, 0.912234428251325905868d0,        &
             0.963971927277913791268d0, 0.993128599185094924786d0/

!					enclosed volume: GQ simplifies
!					to variance of larger volume

      if( (C1.lt.halfe).and.(C2.lt.halfe).and.(C3.lt.halfe) ) then
         d1 = zero
         do 30 i = 1, NG
            xi = Dx*(one + z(i))
            d2 = zero
            do 20 j = 1, NG
               yj = Dy*(one + z(j))
               d3 = zero
               do 10 k = 1, NG
                  zk = Dz*(one + z(k))
                  d3 = d3 + w(k)*(one - z(k))*cov(xi,yj,zk)
  10           continue
               d2 = d2 + w(j)*(one-z(j))*d3
  20        continue
            d1 = d1 + w(i)*(one-z(i))*d2
  30     continue
         dcvab3 = eighth*d1
         return
      endif
!					otherwise, non-enclosed volume
      r1 = half*Dx
      r2 = half*Dy
      r3 = half*Dz

      s2 = two*C1
      s1 = s2 - two
      s3 = s2 + two
      v2 = two*C2
      v1 = v2 - two
      v3 = v2 + two
      u2 = two*C3
      u1 = u2 - two
      u3 = u2 + two

      d1 = zero
      do 60 i = 1, NG
         x1  = r1*(z(i) + s1)
         x2  = r1*(z(i) + s2)
         x3  = r1*(z(i) + s3)
         d21 = zero
         d22 = zero
         d23 = zero
         do 50 j = 1, NG
            y1 = r2*(z(j) + v1)
            y2 = r2*(z(j) + v2)
            y3 = r2*(z(j) + v3)
            d31 = zero
            d32 = zero
            d33 = zero
            d34 = zero
            d35 = zero
            d36 = zero
            d37 = zero
            d38 = zero
            d39 = zero
            do 40 k = 1, NG
               z1 = r3*(z(k) + u1)
               z2 = r3*(z(k) + u2)
               z3 = r3*(z(k) + u3)
               dp = one + z(k)
               dm = one - z(k)
               d31 = d31 + w(k)*(dp*cov(x1,y1,z1) + two*cov(x1,y1,z2)     &
                                +dm*cov(x1,y1,z3))
               d32 = d32 + w(k)*(dp*cov(x1,y2,z1) + two*cov(x1,y2,z2)     &
                                +dm*cov(x1,y2,z3))
               d33 = d33 + w(k)*(dp*cov(x1,y3,z1) + two*cov(x1,y3,z2)     &
                                +dm*cov(x1,y3,z3))
               d34 = d34 + w(k)*(dp*cov(x2,y1,z1) + two*cov(x2,y1,z2)     &
                                +dm*cov(x2,y1,z3))
               d35 = d35 + w(k)*(dp*cov(x2,y2,z1) + two*cov(x2,y2,z2)     &
                                +dm*cov(x2,y2,z3))
               d36 = d36 + w(k)*(dp*cov(x2,y3,z1) + two*cov(x2,y3,z2)     &
                                +dm*cov(x2,y3,z3))
               d37 = d37 + w(k)*(dp*cov(x3,y1,z1) + two*cov(x3,y1,z2)     &
                                +dm*cov(x3,y1,z3))
               d38 = d38 + w(k)*(dp*cov(x3,y2,z1) + two*cov(x3,y2,z2)     &
                                +dm*cov(x3,y2,z3))
               d39 = d39 + w(k)*(dp*cov(x3,y3,z1) + two*cov(x3,y3,z2)     &
                                +dm*cov(x3,y3,z3))
  40        continue
            dp = one + z(j)
            dm = one - z(j)
            d21 = d21 + w(j)*(dp*d31 + two*d32 + dm*d33)
            d22 = d22 + w(j)*(dp*d34 + two*d35 + dm*d36)
            d23 = d23 + w(j)*(dp*d37 + two*d38 + dm*d39)
  50     continue
         d1 = d1 + w(i)*((one+z(i))*d21 + two*d22 + (one-z(i))*d23)
  60  continue
      dcvab3 = fv12th*d1

      return
      end


module polarstereo

IMPLICIT NONE

PRIVATE

PUBLIC :: stereo_loc_latlon_to_xy, stereo_loc_xy_to_latlon, stereo_vec_lonlat_to_xy

REAL*8,PARAMETER :: pi      = 3.1415926535898,        &
&                   deg2rad = 0.01745329251994333333, & ! pi/180
&                   rad2deg = 57.29577951308219756114   ! 180/pi

CONTAINS

!=========================================================================================
! N. Jourdain, LGGE, Grenoble, France
! Dec. 2014
!
! Contains :
! ~~~~~~~~   - stereo_loc_latlon_to_xy : location from (lat,lon) to (x,y)
!            - stereo_loc_xy_to_latlon : location from (x,y) to (lat,lon)
!            - stereo_vec_lonlat_to_xy : vector from (lat,lon) proj to (x,y) proj
!
! some standard info :
! ~~~~~~~~~~~~~~~~~~
!  WGS84 - radius: 6378137.0 eccentricity: 0.08181919
!     in Matlab: axes2ecc(6378137.0, 6356752.3142)
!  Hughes ellipsoid - radius: 6378.273 km eccentricity: 0.081816153
!     Used for SSM/I  http://nsidc.org/data/polar_stereo/ps_grids.html
!  International ellipsoid (following Snyder) - radius: 6378388.0 eccentricity: 0.0819919
!
! history : two first subroutines originally written by Andy Bliss as matlab scripts
! ~~~~~~~
!
! Equations from: Map Projections - A Working manual - by J.P. Snyder. 1987
! http://kartoweb.itc.nl/geometrics/Publications/Map%20Projections%20-%20A%20Working%20manual%20-%20by%20J.P.%20Snyder.pdf
! See the section on Polar Stereographic, with a south polar aspect and known phi_c not at the pole.
!
!=========================================================================================

SUBROUTINE stereo_loc_latlon_to_xy(mx,my,lat_in,lon_in,x_out,y_out,RT,ex,lat_true,lon_posy)
!---------------------------------------------------------------------------------------------
! Writen from matlab's function [x,y]=polarstereo_fwd(phi,lambda,a,e,phi_c,lambda_0)
! Purpose: transforms lat/lon data to map coordinates for a polar stereographic system
!---------------------------------------------------------------------------------------------
INTEGER,                  INTENT(IN)  :: mx, my            !- matrix dimensions
REAL*8, DIMENSION(mx,my), INTENT(IN)  :: lat_in, lon_in    !- input longitude and latitude (degrees)
REAL*8, DIMENSION(mx,my), INTENT(OUT) :: x_out, y_out      !- output x,y coordinates
REAL*8, OPTIONAL,         INTENT(IN)  :: RT,             & !- Earth radius (m)                     [default = WGS84]
&                                        ex,             & !- Earth's misshapenness (excentricity) [default = WGS84]
&                                        lat_true,       & !- latitude of true scale in degrees    [default = 71 degS]
&                                        lon_posy          !- meridian in degrees along the positive Y axis of the map [default = 0 deg]
REAL*8, DIMENSION(mx,my)              :: rho, t, lat, lon
REAL*8                                :: t_c, m_c, a, e, lat_c, lon_0
INTEGER                               :: pm

!- Optional arguments :
if ( present(RT)       ) then; a    = RT       ; else; a     = 6378137.0        ; endif
if ( present(ex)       ) then; e    = ex       ; else; e     =       0.08181919 ; endif
if ( present(lat_true) ) then; lat_c= lat_true ; else; lat_c =     -71.0        ; endif
if ( present(lon_posy) ) then; lon_0= lon_posy ; else; lon_0 =       0.0        ; endif

!- input checking :
if ( a .lt. 6.e6 .or. a .gt. 7.e6 ) then
  write(*,*) '~!@#$%^* error : Earth radius is wrong >>>>>> stop !!'
  stop
endif
!-
if ( e .lt. 0.0 .or. e .gt. 1.0 ) then
  write(*,*) '~!@#$%^* error : Earth excentricity is wrong >>>>>> stop !!'
  stop
endif
!-
if ( lat_c .lt. -90.0 .or. lat_c .gt. 90.0 ) then
  write(*,*) '~!@#$%^* error : latitude of true scale is wrong >>>>>> stop !!'
  stop
endif
!-
if ( lon_0 .lt. -180.0 .or. lon_0 .gt. 360.0 ) then
  write(*,*) '~!@#$%^* error : meridian of positive Y axis is wrong >>>>>> stop !!'
  stop
endif

!- convert to radians :
lat(:,:) = deg2rad * lat_in(:,:)
lat_c    = deg2rad * lat_c
lon(:,:) = deg2rad * lon_in(:,:)
lon_0    = deg2rad * lon_0

!- if the standard parallel is in S.Hemi., switch signs.
if ( lat_c .lt. 0.0 ) then
    pm       = -1    ! plus or minus, north lat. or south
    lat(:,:) = -lat(:,:)
    lat_c    = -lat_c
    lon(:,:) = -lon(:,:)
    lon_0    = -lon_0
else
    pm       = 1
endif

!- See Snyder for details.
t(:,:)   = tan(pi/4-lat(:,:)/2) / ( (1-e*sin(lat(:,:))) / (1+e*sin(lat(:,:))) )**(e/2)
t_c      = tan(pi/4-lat_c   /2) / ( (1-e*sin(lat_c   )) / (1+e*sin(lat_c   )) )**(e/2)
m_c      = cos(lat_c) / sqrt( 1 - e**2 * (sin(lat_c))**2 )
rho(:,:) = a * m_c * t(:,:) / t_c  !- true scale at lat lat_c

x_out(:,:) =  pm * rho(:,:) * sin(lon(:,:)-lon_0)
y_out(:,:) = -pm * rho(:,:) * cos(lon(:,:)-lon_0)

END SUBROUTINE stereo_loc_latlon_to_xy

SUBROUTINE stereo_loc_xy_to_latlon(mx,my,x_in,y_in,lat_out,lon_out,RT,ex,lat_true,lon_posy)
!---------------------------------------------------------------------------------------------
! Writen from matlab's function [phi,lambda]=polarstereo_inv(x,y,a,e,phi_c,lambda_0)
! Purpose: transforms lat/lon data to map coordinates for a polar stereographic system
!---------------------------------------------------------------------------------------------
INTEGER,                  INTENT(IN)  :: mx, my            !- matrix dimensions
REAL*8, DIMENSION(mx,my), INTENT(IN)  :: x_in, y_in        !- input x,y coordinates
REAL*8, DIMENSION(mx,my), INTENT(OUT) :: lat_out, lon_out  !- outut longitude and latitude (degrees)
REAL*8, OPTIONAL,         INTENT(IN)  :: RT,             & !- Earth radius (m)                     [default = WGS84]
&                                        ex,             & !- Earth's misshapenness (excentricity) [default = WGS84]
&                                        lat_true,       & !- latitude of true scale in degrees    [default = 71 degS]
&                                        lon_posy          !- meridian in degrees along the positive Y axis of the map [default = 0 deg]
REAL*8, DIMENSION(mx,my)              :: rho, t, chi, x, y
REAL*8                                :: t_c, m_c, a, e, lat_c, lon_0
INTEGER                               :: pm

!- Optional arguments :
if ( present(RT)       ) then; a    = RT       ; else; a     = 6378137.0        ; endif
if ( present(ex)       ) then; e    = ex       ; else; e     =       0.08181919 ; endif
if ( present(lat_true) ) then; lat_c= lat_true ; else; lat_c =     -71.0        ; endif
if ( present(lon_posy) ) then; lon_0= lon_posy ; else; lon_0 =       0.0        ; endif

!- input checking :
if ( a .lt. 6.e6 .or. a .gt. 7.e6 ) then
  write(*,*) '~!@#$%^* error : Earth radius is wrong >>>>>> stop !!'
  stop
endif
!-
if ( e .lt. 0.0 .or. e .gt. 1.0 ) then
  write(*,*) '~!@#$%^* error : Earth excentricity is wrong >>>>>> stop !!'
  stop
endif
!-
if ( lat_c .lt. -90.0 .or. lat_c .gt. 90.0 ) then
  write(*,*) '~!@#$%^* error : latitude of true scale is wrong >>>>>> stop !!'
  stop
endif
!-
if ( lon_0 .lt. -180.0 .or. lon_0 .gt. 360.0 ) then
  write(*,*) '~!@#$%^* error : meridian of positive Y axis is wrong >>>>>> stop !!'
  stop
endif

!- convert to radians :
lat_c    = deg2rad * lat_c
lon_0    = deg2rad * lon_0

!- if the standard parallel is in S.Hemi., switch signs.
if ( lat_c .lt. 0.0 ) then
    pm     = -1    ! plus or minus, north lat. or south
    lat_c  = -lat_c
    lon_0  = -lon_0
    x(:,:) = -x_in(:,:)
    y(:,:) = -y_in(:,:)
else
    pm     = 1
endif

!- See Snyder for details.
t_c      = tan(pi/4-lat_c   /2) / ( (1-e*sin(lat_c   )) / (1+e*sin(lat_c   )) )**(e/2)
m_c      = cos(lat_c) / sqrt( 1 - e**2 * (sin(lat_c))**2 )
rho(:,:) = sqrt(x(:,:)**2+y(:,:)**2)
t(:,:)   = rho(:,:) * t_c / ( a * m_c )

!- iterate to find phi, with a threshold of pi*1e-8
!  phi_alt=pi/2 - 2 * atan(t); %guess for phi
!  phiold=phi_alt+10; %+10 to make sure it executes the while loop
!  while any(abs(phi_alt(:)-phiold(:)) > pi*1e-8)
!     phiold=phi_alt;
!     phi_alt=pi/2 - 2*atan(t.*((1-e*sin(phi_alt))./(1+e*sin(phi_alt))).^(e/2));
!     %add a break in case it doesn't converge?
!  end

chi(:,:) = 0.5*pi - 2 * atan(t(:,:))  !- find lat with a series instead of iterating.

lat_out(:,:) = chi(:,:) + ( (1./2.) * e**2 + (5./24.) * e**4 + ( 1./ 12.) * e**6 + (  13./   360.) * e**8 ) * sin(2*chi(:,:)) &
&                       + (                  (7./48.) * e**4 + (29./240.) * e**6 + ( 811./ 11520.) * e**8 ) * sin(4*chi(:,:)) &
&                       + (                                  + ( 7./120.) * e**6 + (  81./  1120.) * e**8 ) * sin(6*chi(:,:)) &
&                       + (                                                      + (4279./161280.) * e**8 ) * sin(8*chi(:,:))
lon_out(:,:) = lon_0 + atan2(x,-y)

!- correct the signs and phasing :
lat_out(:,:) = pm * lat_out(:,:)
lon_out(:,:) = pm * lon_out(:,:)
lon_out(:,:) = mod(lon_out(:,:)+pi,2*pi)-pi !- want longitude in the range -pi to pi

!- convert back to degrees :
lat_out(:,:) = rad2deg * lat_out(:,:)
lon_out(:,:) = rad2deg * lon_out(:,:)

END SUBROUTINE stereo_loc_xy_to_latlon

SUBROUTINE stereo_vec_lonlat_to_xy(mx,my,lon_in,uzo_in,vme_in,ux_out,vy_out,lon_posy)
!---------------------------------------------------------------------------------------------
! Purpose: Express (zonal,meridional) vector componants as (x,y) componants on the stereographic map
!---------------------------------------------------------------------------------------------
INTEGER,                  INTENT(IN)  :: mx, my            !- matrix dimensions
REAL*8, DIMENSION(mx,my), INTENT(IN)  :: lon_in            !- input longitude (deg East)
REAL*8, DIMENSION(mx,my), INTENT(IN)  :: uzo_in, vme_in    !- vector on input lon,lat grid
REAL*8, DIMENSION(mx,my), INTENT(OUT) :: ux_out, vy_out    !- vector on output x,y grid
REAL*8, OPTIONAL,         INTENT(IN)  :: lon_posy          !- meridian in degrees along the positive Y axis of the map [default = 0 deg]
REAL*8, DIMENSION(mx,my)              :: lon
REAL*8                                :: lon_0

!- Optional arguments :
if ( present(lon_posy) ) then; lon_0= lon_posy ; else; lon_0 =       0.0        ; endif

!- input checking :
if ( lon_0 .lt. -180.0 .or. lon_0 .gt. 360.0 ) then
  write(*,*) '~!@#$%^* error : meridian of positive Y axis is wrong >>>>>> stop !!'
  stop
endif

!- convert to radians :
lon(:,:) = deg2rad * lon_in(:,:)
lon_0    = deg2rad * lon_0

ux_out(:,:) =   uzo_in(:,:) * cos(lon(:,:)-lon_0) + vme_in(:,:) * sin(lon(:,:)-lon_0)
vy_out(:,:) = - uzo_in(:,:) * sin(lon(:,:)-lon_0) + vme_in(:,:) * cos(lon(:,:)-lon_0)

END SUBROUTINE stereo_vec_lonlat_to_xy

!=======================================================
end module polarstereo


      SUBROUTINE SPLAAN(N, X, Y, B, C, D)                               ABSH1042
C***********************************************************************ABSH1043
C*****SPLAAN EVALUATES THE COEFFICIENTS FOR A CUBIC INTERPOLATING      *ABSH1044
C*****SPLINE FOR WHICH S'(X1)=0.                                       *ABSH1045
C*****REFERENCES:                                                      *ABSH1046
C*****MODIFICATION OF SPLINE ROUTINE IN BELOW REFERENCE BY R.M.WIELAND *ABSH1047
C*****FOR S'(X1)=0.                                                    *ABSH1048
C*****G.E.FORSYTHE,M.A.MALCOLM,C.B.MOLER, COMPUTER METHODS FOR MATHE-  *ABSH1049
C***** MATICAL COMPUTATIONS, PRENTICE-HALL, 1977, P.76.                *ABSH1050
C*****LAST REVISION: 6/81 R.M.WIELAND AND W.A.HOULBERG ORNL.           *ABSH1051
C*****CALCULATED PARAMETERS:                                           *ABSH1052
C*****B(I),C(I),D(I)-ARRAYS OF N SPLINE COEFFICIENTS.                  *ABSH1053
C*****Y(I)=S(X(I)).                                                    *ABSH1054
C*****B(I)=S'(X(I)).                                                   *ABSH1055
C*****C(I)=S''(X(I)).                                                  *ABSH1056
C*****D(I)=S'''(X(I)).                                                 *ABSH1057
C*****INPUT PARAMETERS:                                                *ABSH1058
C*****N-NUMBER OF DATA POINTS OR KNOTS-(N.GE.2).                       *ABSH1059
C*****X(I)-ABSCISSAS OF THE KNOTS IN STRICTLY INCREASING ORDER.        *ABSH1060
C*****Y(I)-ORDINATES OF THE KNOTS.                                     *ABSH1061
C*****OTHER COMMENTS:                                                  *ABSH1062
C*****S(X)=Y(I)+B(I)*(X-X(I))+C(I)*(X-X(I))**2+D(I)*(X-X(I))**3        *ABSH1063
C*****FOR X(I).LE.X.LE.X(I+1)                                          *ABSH1064
C*****SEVAL CAN BE USED TO EVALUATE THE SPLINE.                        *ABSH1065
C*****SPEVAL CAN BE USED TO EVALUATE THE DERIVATIVE OF THE SPLINE.     *ABSH1066
C***********************************************************************ABSH1067
      DIMENSION X(N), Y(N), B(N), C(N), D(N)                            ABSH1068
      NM1 = N - 1                                                       ABSH1069
      IF (N.LT.2) RETURN                                                ABSH1070
      IF (N.LT.3) GO TO 60                                              ABSH1071
C*****SET UP TRIDIAGONAL SYSTEM:                                        ABSH1072
C*****B=DIAGONAL, D=OFFDIAGONAL, C=RIGHT HAND SIDE.                     ABSH1073
      D(1) = X(2) - X(1)                                                ABSH1074
      C(2) = (Y(2)-Y(1))/D(1)                                           ABSH1075
      DO 10 I=2,NM1                                                     ABSH1076
           D(I) = X(I+1) - X(I)                                         ABSH1077
           B(I) = 2.0*(D(I-1)+D(I))                                     ABSH1078
           C(I+1) = (Y(I+1)-Y(I))/D(I)                                  ABSH1079
           C(I) = C(I+1) - C(I)                                         ABSH1080
   10 CONTINUE                                                          ABSH1081
C*****END CONDITIONS:                                                   ABSH1082
C*****THIRD DERIVATIVES AT 1 AND N OBTAINED FROM DIVIDED DIFFERENCES.   ABSH1083
      B(1) = 2.0*D(1)                                                   ABSH1084
      B(N) = -D(N-1)                                                    ABSH1085
      C(1) = 0.0                                                        ABSH1086
      C(N) = 0.0                                                        ABSH1087
      IF (N.EQ.3) GO TO 20                                              ABSH1088
      C(1) = (Y(2)-Y(1))/D(1)                                           ABSH1089
      C(N) = C(N-1)/(X(N)-X(N-2)) - C(N-2)/(X(N-1)-X(N-3))              ABSH1090
      C(N) = -C(N)*D(N-1)**2/(X(N)-X(N-3))                              ABSH1091
C*****FORWARD ELIMINATION.                                              ABSH1092
   20 DO 30 I=2,N                                                       ABSH1093
           T = D(I-1)/B(I-1)                                            ABSH1094
           B(I) = B(I) - T*D(I-1)                                       ABSH1095
           C(I) = C(I) - T*C(I-1)                                       ABSH1096
   30 CONTINUE                                                          ABSH1097
C*****BACK SUBSTITUTION.                                                ABSH1098
      C(N) = C(N)/B(N)                                                  ABSH1099
      DO 40 IB=1,NM1                                                    ABSH1100
           I = N - IB                                                   ABSH1101
           C(I) = (C(I)-D(I)*C(I+1))/B(I)                               ABSH1102
   40 CONTINUE                                                          ABSH1103
C*****C(I) IS NOW THE SIGMA(I) OF THE TEXT.                             ABSH1104
C*****COMPUTE POLYNOMIAL COEFFICIENTS.                                  ABSH1105
      B(N) = (Y(N)-Y(NM1))/D(NM1) + D(NM1)*(C(NM1)+2.0*C(N))            ABSH1106
      DO 50 I=1,NM1                                                     ABSH1107
           B(I) = (Y(I+1)-Y(I))/D(I) - D(I)*(C(I+1)+2.0*C(I))           ABSH1108
           D(I) = (C(I+1)-C(I))/D(I)                                    ABSH1109
           C(I) = 3.0*C(I)                                              ABSH1110
   50 CONTINUE                                                          ABSH1111
      C(N) = 3.0*C(N)                                                   ABSH1112
      D(N) = D(N-1)                                                     ABSH1113
      RETURN                                                            ABSH1114
C*****COEFFICIENTS FOR N=2.                                             ABSH1115
   60 B(1) = (Y(2)-Y(1))/(X(2)-X(1))                                    ABSH1116
      C(1) = 0.0                                                        ABSH1117
      D(1) = 0.0                                                        ABSH1118
      B(2) = B(1)                                                       ABSH1119
      C(2) = 0.0                                                        ABSH1120
      D(2) = 0.0                                                        ABSH1121
      RETURN                                                            ABSH1122
      END                                                               ABSH1123


!==============================================================================!
  real function Tau_Wall_Rough_Walls(turb, dens, u_tau, u_tan, wall_dist, z_o)
!------------------------------------------------------------------------------!
!   Calculates tau_wall for rough walls.                                       !
!------------------------------------------------------------------------------!
!----------------------------------[Modules]-----------------------------------!
  use Turb_Mod, only: Turb_Type, kappa
!------------------------------------------------------------------------------!
  implicit none
!---------------------------------[Arguments]----------------------------------!
  type(Turb_Type) :: turb
  real            :: dens, u_tau, u_tan, wall_dist, z_o
!==============================================================================!

  Tau_Wall_Rough_Walls = dens * kappa * u_tau * u_tan  &
                       / log(((wall_dist + z_o) / z_o))

  end function


      subroutine sheart(q,sslope,shear)
c
c     + + + PURPOSE + + +
c     Compute top rill width adjustments, Chezy's coefficient for rill
c     flow, flow depth, wetted area, wetted perimeter, hydraulic
c     radius, and shear stress at the end of the slope
c
c     Called from PARAM
c     Author(s): Flanagan
c     Reference in User Guide:
c
c     + + + KEYWORDS + + +
c
c     + + + PARAMETERS + + +
      include 'pmxnsl.inc'
      include 'pmxpln.inc'
      include 'pmxtls.inc'
      include 'pmxtil.inc'
      include 'pmxhil.inc'
      include 'pmxelm.inc'
c
c     + + + ARGUMENT DECLARATIONS + + +
      real q, sslope, shear
c
c     + + + ARGUMENT DEFINITIONS + + +
c
c     q      : flow discharge (m**3/s)
c     sslope : channel slope (m/m)
c     shear  : flow shear stress (Pa)
c
c     + + + COMMON BLOCKS + + +
c
      include 'cclim.inc'
      include 'cconsta.inc'
c
      include 'cends.inc'
c      modify: wdhtop(mxplan)
c
      include 'cffact.inc'
c
      include 'cslpopt.inc'
c     modify: dpthch(mxplan)
c
      include 'csolvar.inc'
      include 'cstruc.inc'
      include 'cupdate.inc'
c
c     + + + LOCAL VARIABLES + + +
      real dz, tol, u, chezch, sinang, wdthck, wp,
     1    xsarea,dpthch
c
c     + + + LOCAL DEFINITIONS + + +
c
c      u      : portion of uniform flow equation
c      wdthck : test width to check against to see if wider than
c               current rill width
c      chezch : Chezy C - roughness factor
c      dz     : trial valve for channel depth (m)
c      xsarea : cross sectional area of the flow (m**2)
c      wp     : wetted perimeter of the flow (m)
c      hydrad : hydraulic radius (m)
c      sinang : sine of slope angle
c      tol    : tolerance value
c
      save
c
c********************************************************************
c
c     Tolerance value changed by Baffaut, 1996.  dcf 3/97
c     tol = 5.0e-05
      tol = 5.0e-06
      q = abs(q)
c
c     SSLOPE value changed by Baffaut, 1996.  dcf 3/97
c     if (sslope.le.0.0) sslope = 0.00001
      if (sslope.le.0.0) sslope = 0.000001
c
c     compute top rill width (wdhtop). Note that when tillage occurs
c     top rill wdhtop is set to zero in SR CONTIN
c
c     Using Gilley's relationship
c
      if (rwflag(iplane).eq.1) then
        wdthck = 1.13 * q ** 0.303
        if (wdhtop(iplane).lt.wdthck) wdhtop(iplane) = wdthck
      end if
c
      if (wdhtop(iplane).gt.rspace(iplane)) wdhtop(iplane) =
     1    rspace(iplane)
c
c     compute Chezy's coefficient:
c
      chezch = sqrt(8.0*accgav/frctrl(iplane))
c
c     compute rill flow depth (dpthch(iplane)). This is an iterative
c     process to solve the uniform flow equation:
c
c     dpthch(iplane)=((Q/chezch/sqrt(sslope)**
c                    (2/3)/width)*(width+2*dpthch(iplane))**(1/3)
c
      if (q.le.0.) then
        dpthch = 0.0
      else
        u = (q/chezch/sqrt(sslope)) ** (2./3.) / wdhtop(iplane)
        dpthch = 0.2 * q ** .36
   10   dz = dpthch
        dpthch = u * (wdhtop(iplane)+dz+dz) ** (1./3.)
        if (abs(dz/dpthch-1.).gt.tol) go to 10
      end if
c
c     compute wetted area (xsarea), wetted perimeter (wp), and
c     hydraulic radius (hydrad):
c
      xsarea = dpthch * wdhtop(iplane)
      wp = wdhtop(iplane) + 2.0 * dpthch
c
c     check for rill width of 0 causing 0 wetted perimeter
c     4-24-2008 jrf
c      if (wp.ne.0.0) then
         hydrad(iplane) = xsarea / wp
c      else
c         hydrad(iplane) = 0.0
c      endif
c
c     compute shear stress:
c
c     Correction made to compute shear stress using the SINE of the
c     slope angle, not the tangent of the slope angle as has previously
c     been computed.  This should only impact results greatly on steeper
c     hillslopes.   dcf   1/21/93
c     shears=wtdens*sslope*hydrad*frcsol(iplane)/frctrl(iplane)
c
      sinang = sin(atan(sslope))
      shear = wtdens * sinang * hydrad(iplane) * frcsol(iplane) /
     1    frctrl(iplane)
      return
      end