c
c  . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
c  .                                                             .
c  .                  copyright (c) 1998 by UCAR                 .
c  .                                                             .
c  .       University Corporation for Atmospheric Research       .
c  .                                                             .
c  .                      all rights reserved                    .
c  .                                                             .
c  .                                                             .
c  .                         SPHEREPACK                          .
c  .                                                             .
c  . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
c
c
C*PL*ERROR* Comment line too long
c ... file sshifte.f contains code and documentation for subroutine sshifte
c     and its' initialization subroutine sshifti
c
c ... required files off spherepack3.0
c
c     hrfft.f
c
C*PL*ERROR* Comment line too long
c     subroutine sshifte(ioff,nlon,nlat,goff,greg,wsav,lsav,work,lwork,ier)
c
c *** purpose
c
c     subroutine sshifte does a highly accurate 1/2 grid increment shift
C*PL*ERROR* Comment line too long
c     in both longitude and latitude of equally spaced data on the sphere.
c     data is transferred between the nlon by nlat "offset grid" in goff
C*PL*ERROR* Comment line too long
c     (which excludes poles) and the nlon by nlat+1 "regular grid" in greg
C*PL*ERROR* Comment line too long
c     (which includes poles).  the transfer can go from goff to greg or from
C*PL*ERROR* Comment line too long
c     greg to goff (see ioff).  the grids which underly goff and greg are
C*PL*ERROR* Comment line too long
c     described below.  the north and south poles are at latitude 0.5*pi and
c     -0.5*pi radians respectively where pi = 4.*atan(1.).
c
c *** grid descriptions
c
c     let dlon = (pi+pi)/nlon and dlat = pi/nlat be the uniform grid
c     increments in longitude and latitude
c
c     offset grid
c
C*PL*ERROR* Comment line too long
c     the "1/2 increment offset" grid (long(j),lat(i)) on which goff(j,i)
c     is given (ioff=0) or generated (ioff=1) is
c
c          long(j) =0.5*dlon + (j-1)*dlon  (j=1,...,nlon)
c
c     and
c
c          lat(i) = -0.5*pi + 0.5*dlat + (i-1)*dlat (i=1,...,nlat)
c
C*PL*ERROR* Comment line too long
c     the data in goff is "shifted" one half a grid increment in longitude
c     and latitude and excludes the poles.  each goff(j,1) is given at
C*PL*ERROR* Comment line too long
c     latitude -0.5*pi+0.5*dlat and goff(j,nlat) is given at 0.5*pi-0.5*dlat
C*PL*ERROR* Comment line too long
c     (1/2 a grid increment away from the poles).  each goff(1,i),goff(nlon,i)
c     is given at longitude 0.5*dlon and 2.*pi-0.5*dlon.
c
c     regular grid
c
c     let dlat,dlon be as above.  then the nlon by nlat+1 grid on which
c     greg(j,i) is generated (ioff=0) or given (ioff=1) is given by
c
c          lone(j) = (j-1)*dlon (j=1,...,nlon)
c
c      and
c
c          late(i) = -0.5*pi + (i-1)*dlat (i=1,...,nlat+1)
c
C*PL*ERROR* Comment line too long
c     values in greg include the poles and start at zero degrees longitude.
c
c *** remark
c
C*PL*ERROR* Comment line too long
c     subroutine sshifte can be used in conjunction with subroutine trssph
C*PL*ERROR* Comment line too long
c     when transferring data from an equally spaced "1/2 increment offset"
C*PL*ERROR* Comment line too long
c     grid to a gaussian or equally spaced grid (which includes poles) of
c     any resolution.  this problem (personal communication with dennis
c     shea) is encountered in geophysical modeling and data analysis.
c
c *** method
c
c     fast fourier transform software from spherepack2 and trigonometric
c     identities are used to accurately "shift" periodic vectors half a
c     grid increment in latitude and longitude.  latitudinal shifts are
C*PL*ERROR* Comment line too long
c     accomplished by setting periodic 2*nlat vectors over the pole for each
C*PL*ERROR* Comment line too long
c     longitude.  when nlon is odd, this requires an additional longitude
C*PL*ERROR* Comment line too long
c     shift.  longitudinal shifts are then executed for each shifted latitude.
c     when necessary (ioff=0) poles are obtained by averaging the nlon
c     shifted polar values.
c
c *** required files from spherepack3.0
c
c     hrfft.f
c
c *** argument description
c
c ... ioff
c
c     ioff = 0 if values on the offset grid in goff are given and values
c              on the regular grid in greg are to be generated.
c
C*PL*ERROR* Comment line too long
c     ioff = 1 if values on the regular grid in greg are given and values
c              on the offset grid in goff are to be generated.
c
c ... nlon
c
c     the number of longitude points on both the "offset" and "regular"
c     uniform grid in longitude (see "grid description" above).  nlon
C*PL*ERROR* Comment line too long
c     is also the first dimension of array goff and greg.  nlon determines
C*PL*ERROR* Comment line too long
c     the grid increment in longitude as dlon = 2.*pi/nlon.  for example,
C*PL*ERROR* Comment line too long
c     nlon = 144 for a 2.5 degree grid.  nlon can be even or odd and must
c     be greater than or equal to 4.  the efficiency of the computation
c     is improved when nlon is a product of small primes.
c
c ... nlat
c
C*PL*ERROR* Comment line too long
c     the number of latitude points on the "offset" uniform grid.  nlat+1
C*PL*ERROR* Comment line too long
c     is the number of latitude points on the "regular" uniform grid (see
C*PL*ERROR* Comment line too long
c     "grid description" above).  nlat is the second dimension of array goff.
C*PL*ERROR* Comment line too long
c     nlat+1 must be the second dimension of the array greg in the program
c     calling sshifte.  nlat determines the grid in latitude as pi/nlat.
C*PL*ERROR* Comment line too long
c     for example, nlat = 36 for a five degree grid.  nlat must be at least 3.
c
c ... goff
c
c     a nlon by nlat array that contains data on the offset grid
c     described above.  goff is a given input argument if ioff=0.
c     goff is a generated output argument if ioff=1.
c
c ... greg
c
c     a nlon by nlat+1 array that contains data on the regular grid
c     described above.  greg is a given input argument if ioff=1.
c     greg is a generated output argument if ioff=0.
c
c ... wsav
c
c     a real saved work space array that must be initialized by calling
C*PL*ERROR* Comment line too long
c     subroutine sshifti(ioff,nlon,nlat,wsav,ier) before calling sshifte.
c     wsav can then be used repeatedly by sshifte as long as ioff, nlon,
c     and nlat do not change.  this bypasses redundant computations and
c     saves time.  undetectable errors will result if sshifte is called
c     without initializing wsav whenever ioff, nlon, or nlat change.
c
c ... lsav
c
C*PL*ERROR* Comment line too long
c     the length of the saved work space wsav in the routine calling sshifte
C*PL*ERROR* Comment line too long
c     and sshifti.  lsave must be greater than or equal to 2*(2*nlat+nlon+16).
c
c ... work
c
c     a real unsaved work space
c
c ... lwork
c
C*PL*ERROR* Comment line too long
c     the length of the unsaved work space in the routine calling sshifte
C*PL*ERROR* Comment line too long
c     lwork must be greater than or equal to 2*nlon*(nlat+1) if nlon is even.
C*PL*ERROR* Comment line too long
c     lwork must be greater than or equal to nlon*(5*nlat+1) if nlon is odd.
c
c ... ier
c
c     indicates errors in input parameters
c
c     = 0 if no errors are detected
c
c     = 1 if ioff is not equal to 0 or 1
c
c     = 1 if nlon < 4
c
c     = 2 if nlat < 3
c
c     = 3 if lsave < 2*(nlon+2*nlat+16)
c
c     = 4 if lwork < 2*nlon*(nlat+1) for nlon even or
c            lwork < nlon*(5*nlat+1) for nlon odd
c
c *** end of sshifte documentation
c
c     subroutine sshifti(ioff,nlon,nlat,lsav,wsav,ier)
c
c     subroutine sshifti initializes the saved work space wsav
c     for ioff and nlon and nlat (see documentation for sshifte).
c     sshifti must be called before sshifte whenever ioff or nlon
c     or nlat change.
c
c ... ier
c
c     = 0 if no errors with input arguments
c
c     = 1 if ioff is not 0 or 1
c
c     = 2 if nlon < 4
c
c     = 3 if nlat < 3
c
c     = 4 if lsav < 2*(2*nlat+nlon+16)
c
c *** end of sshifti documentation
c
      SUBROUTINE DSSHIFTE(IOFF,NLON,NLAT,GOFF,GREG,WSAV,LSAV,WRK,LWRK,
     +                   IER)
      IMPLICIT NONE
      INTEGER IOFF,NLON,NLAT,N2,NR,NLAT2,NLATP1,LSAV,LWRK,I1,I2,IER
      DOUBLE PRECISION GOFF(NLON,NLAT),GREG(NLON,*),WSAV(LSAV),WRK(LWRK)
c
c     check input parameters
c
      IER = 1
      IF (IOFF* (IOFF-1).NE.0) RETURN
      IER = 2
      IF (NLON.LT.4) RETURN
      IER = 3
      IF (NLAT.LT.3) RETURN
      IER = 4
      IF (LSAV.LT.2* (2*NLAT+NLON+16)) RETURN
      IER = 5
      N2 = (NLON+1)/2
      IF (2*N2.EQ.NLON) THEN
          IF (LWRK.LT.2*NLON* (NLAT+1)) RETURN
          I1 = 1
          NR = N2
      ELSE
          IF (LWRK.LT.NLON* (5*NLAT+1)) RETURN
          I1 = 1 + 2*NLAT*NLON
          NR = NLON
      END IF
      IER = 0
      NLAT2 = NLAT + NLAT
      I2 = I1 + (NLAT+1)*NLON
      IF (IOFF.EQ.0) THEN
          CALL DSHFTOFF(NLON,NLAT,GOFF,GREG,WSAV,NR,NLAT2,WRK,WRK(I1),
     +                 WRK(I2))
      ELSE
          NLATP1 = NLAT + 1
          CALL DSHFTREG(NLON,NLAT,GOFF,GREG,WSAV,NR,NLAT2,NLATP1,WRK,
     +                 WRK(I1),WRK(I2))
      END IF
      END

      SUBROUTINE DSHFTOFF(NLON,NLAT,GOFF,GREG,WSAV,NR,NLAT2,RLAT,RLON,
     +                   WRK)
c
c     shift offset grid to regular grid, i.e.,
c     goff is given, greg is to be generated
c
      IMPLICIT NONE
      INTEGER NLON,NLAT,NLAT2,N2,NR,J,I,JS,ISAV
      DOUBLE PRECISION GOFF(NLON,NLAT),GREG(NLON,NLAT+1)
      DOUBLE PRECISION RLAT(NR,NLAT2),RLON(NLAT,NLON)
      DOUBLE PRECISION WSAV(*),WRK(*)
      DOUBLE PRECISION GNORTH,GSOUTH

      ISAV = 4*NLAT + 17
      N2 = (NLON+1)/2
c
c     execute full circle latitude shifts for nlon odd or even
c
      IF (2*N2.GT.NLON) THEN
c
c     odd number of longitudes
c
          DO I = 1,NLAT
              DO J = 1,NLON
                  RLON(I,J) = GOFF(J,I)
              END DO
          END DO
c
c       half shift in longitude
c
          CALL DSHIFTH(NLAT,NLON,RLON,WSAV(ISAV),WRK)
c
c       set full 2*nlat circles in rlat using shifted values in rlon
c
          DO J = 1,N2 - 1
              JS = J + N2
              DO I = 1,NLAT
                  RLAT(J,I) = GOFF(J,I)
                  RLAT(J,NLAT+I) = RLON(NLAT+1-I,JS)
              END DO
          END DO
          DO J = N2,NLON
              JS = J - N2 + 1
              DO I = 1,NLAT
                  RLAT(J,I) = GOFF(J,I)
                  RLAT(J,NLAT+I) = RLON(NLAT+1-I,JS)
              END DO
          END DO
c
c       shift the nlon rlat vectors one half latitude grid
c
          CALL DSHIFTH(NLON,NLAT2,RLAT,WSAV,WRK)
c
c       set nonpole values in greg and average for poles
c
          GNORTH = 0.0D0
          GSOUTH = 0.0D0
          DO J = 1,NLON
              GNORTH = GNORTH + RLAT(J,1)
              GSOUTH = GSOUTH + RLAT(J,NLAT+1)
              DO I = 2,NLAT
                  GREG(J,I) = RLAT(J,I)
              END DO
          END DO
          GNORTH = GNORTH/NLON
          GSOUTH = GSOUTH/NLON

      ELSE
c
c     even number of longitudes (no initial longitude shift necessary)
C*PL*ERROR* Comment line too long
c     set full 2*nlat circles (over poles) for each longitude pair (j,js)
c
          DO J = 1,N2
              JS = N2 + J
              DO I = 1,NLAT
                  RLAT(J,I) = GOFF(J,I)
                  RLAT(J,NLAT+I) = GOFF(JS,NLAT+1-I)
              END DO
          END DO
c
c       shift the n2=(nlon+1)/2 rlat vectors one half latitude grid
c
          CALL DSHIFTH(N2,NLAT2,RLAT,WSAV,WRK)
c
c       set nonpole values in greg and average poles
c
          GNORTH = 0.0D0
          GSOUTH = 0.0D0
          DO J = 1,N2
              JS = N2 + J
              GNORTH = GNORTH + RLAT(J,1)
              GSOUTH = GSOUTH + RLAT(J,NLAT+1)
              DO I = 2,NLAT
                  GREG(J,I) = RLAT(J,I)
                  GREG(JS,I) = RLAT(J,NLAT2-I+2)
              END DO
          END DO
          GNORTH = GNORTH/N2
          GSOUTH = GSOUTH/N2
      END IF
c
c     set poles
c
      DO J = 1,NLON
          GREG(J,1) = GNORTH
          GREG(J,NLAT+1) = GSOUTH
      END DO
c
c     execute full circle longitude shift
c
      DO J = 1,NLON
          DO I = 1,NLAT
              RLON(I,J) = GREG(J,I)
          END DO
      END DO
      CALL DSHIFTH(NLAT,NLON,RLON,WSAV(ISAV),WRK)
      DO J = 1,NLON
          DO I = 2,NLAT
              GREG(J,I) = RLON(I,J)
          END DO
      END DO
      END

      SUBROUTINE DSHFTREG(NLON,NLAT,GOFF,GREG,WSAV,NR,NLAT2,NLATP1,RLAT,
     +                   RLON,WRK)
c
c     shift regular grid to offset grid, i.e.,
c     greg is given, goff is to be generated
c
      IMPLICIT NONE
      INTEGER NLON,NLAT,NLAT2,NLATP1,N2,NR,J,I,JS,ISAV
      DOUBLE PRECISION GOFF(NLON,NLAT),GREG(NLON,NLATP1)
      DOUBLE PRECISION RLAT(NR,NLAT2),RLON(NLATP1,NLON)
      DOUBLE PRECISION WSAV(*),WRK(*)

      ISAV = 4*NLAT + 17
      N2 = (NLON+1)/2
c
c     execute full circle latitude shifts for nlon odd or even
c
      IF (2*N2.GT.NLON) THEN
c
c     odd number of longitudes
c
          DO I = 1,NLAT + 1
              DO J = 1,NLON
                  RLON(I,J) = GREG(J,I)
              END DO
          END DO
c
c       half shift in longitude in rlon
c
          CALL DSHIFTH(NLAT+1,NLON,RLON,WSAV(ISAV),WRK)
c
c       set full 2*nlat circles in rlat using shifted values in rlon
c
          DO J = 1,N2
              JS = J + N2 - 1
              RLAT(J,1) = GREG(J,1)
              DO I = 2,NLAT
                  RLAT(J,I) = GREG(J,I)
                  RLAT(J,NLAT+I) = RLON(NLAT+2-I,JS)
              END DO
              RLAT(J,NLAT+1) = GREG(J,NLAT+1)
          END DO
          DO J = N2 + 1,NLON
              JS = J - N2
              RLAT(J,1) = GREG(J,1)
              DO I = 2,NLAT
                  RLAT(J,I) = GREG(J,I)
                  RLAT(J,NLAT+I) = RLON(NLAT+2-I,JS)
              END DO
              RLAT(J,NLAT+1) = GREG(J,NLAT+1)
          END DO
c
c       shift the nlon rlat vectors one halflatitude grid
c
          CALL DSHIFTH(NLON,NLAT2,RLAT,WSAV,WRK)
c
c       set values in goff
c
          DO J = 1,NLON
              DO I = 1,NLAT
                  GOFF(J,I) = RLAT(J,I)
              END DO
          END DO

      ELSE
c
c     even number of longitudes (no initial longitude shift necessary)
C*PL*ERROR* Comment line too long
c     set full 2*nlat circles (over poles) for each longitude pair (j,js)
c
          DO J = 1,N2
              JS = N2 + J
              RLAT(J,1) = GREG(J,1)
              DO I = 2,NLAT
                  RLAT(J,I) = GREG(J,I)
                  RLAT(J,NLAT+I) = GREG(JS,NLAT+2-I)
              END DO
              RLAT(J,NLAT+1) = GREG(J,NLAT+1)
          END DO
c
c       shift the n2=(nlon+1)/2 rlat vectors one half latitude grid
c
          CALL DSHIFTH(N2,NLAT2,RLAT,WSAV,WRK)
c
c       set values in goff
c
          DO J = 1,N2
              JS = N2 + J
              DO I = 1,NLAT
                  GOFF(J,I) = RLAT(J,I)
                  GOFF(JS,I) = RLAT(J,NLAT2+1-I)
              END DO
          END DO
      END IF
c
c     execute full circle longitude shift for all latitude circles
c
      DO J = 1,NLON
          DO I = 1,NLAT
              RLON(I,J) = GOFF(J,I)
          END DO
      END DO
      CALL DSHIFTH(NLAT+1,NLON,RLON,WSAV(ISAV),WRK)
      DO J = 1,NLON
          DO I = 1,NLAT
              GOFF(J,I) = RLON(I,J)
          END DO
      END DO
      END

      SUBROUTINE DSSHIFTI(IOFF,NLON,NLAT,LSAV,WSAV,IER)
      INTEGER IOFF,NLAT,NLON,NLAT2,ISAV,IER
      DOUBLE PRECISION WSAV(LSAV)
      DOUBLE PRECISION PI,DLAT,DLON,DP

      IER = 1
      IF (IOFF* (IOFF-1).NE.0) RETURN
      IER = 2
      IF (NLON.LT.4) RETURN
      IER = 3
      IF (NLAT.LT.3) RETURN
      IER = 4
      IF (LSAV.LT.2* (2*NLAT+NLON+16)) RETURN
      IER = 0
      PI = 4.0D0*ATAN(1.0D0)
c
c     set lat,long increments
c
      DLAT = PI/NLAT
      DLON = (PI+PI)/NLON
c
c     initialize wsav for left or right latitude shifts
c
      IF (IOFF.EQ.0) THEN
          DP = -0.5D0*DLAT
      ELSE
          DP = 0.5D0*DLAT
      END IF
      NLAT2 = NLAT + NLAT
      CALL DSHIFTHI(NLAT2,DP,WSAV)
c
c     initialize wsav for left or right longitude shifts
c
      IF (IOFF.EQ.0) THEN
          DP = -0.5D0*DLON
      ELSE
          DP = 0.5D0*DLON
      END IF
      ISAV = 4*NLAT + 17
      CALL DSHIFTHI(NLON,DP,WSAV(ISAV))
      RETURN
      END

      SUBROUTINE DSHIFTH(M,N,R,WSAV,WORK)
      IMPLICIT NONE
      INTEGER M,N,N2,K,L
      DOUBLE PRECISION R(M,N),WSAV(*),WORK(*),R2KM2,R2KM1

      N2 = (N+1)/2
c
c     compute fourier coefficients for r on shifted grid
c
      CALL DHRFFTF(M,N,R,M,WSAV(N+2),WORK)
      DO L = 1,M
          DO K = 2,N2
              R2KM2 = R(L,K+K-2)
              R2KM1 = R(L,K+K-1)
              R(L,K+K-2) = R2KM2*WSAV(N2+K) - R2KM1*WSAV(K)
              R(L,K+K-1) = R2KM2*WSAV(K) + R2KM1*WSAV(N2+K)
          END DO
      END DO
c
c     shift r with fourier synthesis and normalization
c
      CALL DHRFFTB(M,N,R,M,WSAV(N+2),WORK)
      DO L = 1,M
          DO K = 1,N
              R(L,K) = R(L,K)/N
          END DO
      END DO
      RETURN
      END

      SUBROUTINE DSHIFTHI(N,DP,WSAV)
c
c     initialize wsav for subroutine shifth
c
      IMPLICIT NONE
      INTEGER N,N2,K
      DOUBLE PRECISION WSAV(*),DP

      N2 = (N+1)/2
      DO K = 2,N2
          WSAV(K) = SIN((K-1)*DP)
          WSAV(K+N2) = COS((K-1)*DP)
      END DO
      CALL DHRFFTI(N,WSAV(N+2))
      RETURN
      END


module physical_constants

  implicit none

  double precision, parameter :: PI = 3.141592653589793238462643383279502884197d0
  double precision, parameter :: CM_PER_FM = 1.0d-13

  ! From PDG 2014
  double precision, parameter :: CLIGHT  = 299792458d2 ! cm/s
  double precision, parameter :: KB_MeV_PER_K = 8.6173324d-11 ! MeV/K
  double precision, parameter :: GNEWTON  = 6.67384d-8 ! cm^3/g/s^2
  double precision, parameter :: ERG_PER_MeV = 1.602176565d-6
  double precision, parameter :: N_AVO = 6.02214129d23

  ! From AME 2012 Atomic Mass Evaluation
  double precision, parameter :: GRAM_PER_AMU = 1660538.921d-30 ! g

  double precision, parameter :: ev2erg  = 1.60217648740d-12
  double precision, parameter :: mev2erg = ev2erg*1.0d6
  double precision, parameter :: mev2gr  = mev2erg/CLIGHT**2

  double precision, parameter :: mn = 1.67492721184d-24
  double precision, parameter :: mp = 1.67262163783d-24
  double precision, parameter :: me = 9.1093821545d-28

end module physical_constants


      SUBROUTINE MB02TD( NORM, N, HNORM, H, LDH, IPIV, RCOND, IWORK,
     $                   DWORK, INFO )
C
C     SLICOT RELEASE 5.7.
C
C     Copyright (c) 2002-2020 NICONET e.V.
C
C     PURPOSE
C
C     To estimate the reciprocal of the condition number of an upper
C     Hessenberg matrix H, in either the 1-norm or the infinity-norm,
C     using the LU factorization computed by MB02SD.
C
C     ARGUMENTS
C
C     Mode Parameters
C
C     NORM    CHARACTER*1
C             Specifies whether the 1-norm condition number or the
C             infinity-norm condition number is required:
C             = '1' or 'O':  1-norm;
C             = 'I':         Infinity-norm.
C
C     Input/Output Parameters
C
C     N       (input) INTEGER
C             The order of the matrix H.  N >= 0.
C
C     HNORM   (input) DOUBLE PRECISION
C             If NORM = '1' or 'O', the 1-norm of the original matrix H.
C             If NORM = 'I', the infinity-norm of the original matrix H.
C
C     H       (input) DOUBLE PRECISION array, dimension (LDH,N)
C             The factors L and U from the factorization H = P*L*U
C             as computed by MB02SD.
C
C     LDH     INTEGER
C             The leading dimension of the array H.  LDH >= max(1,N).
C
C     IPIV    (input) INTEGER array, dimension (N)
C             The pivot indices; for 1 <= i <= N, row i of the matrix
C             was interchanged with row IPIV(i).
C
C     RCOND   (output) DOUBLE PRECISION
C             The reciprocal of the condition number of the matrix H,
C             computed as RCOND = 1/(norm(H) * norm(inv(H))).
C
C     Workspace
C
C     IWORK   INTEGER array, dimension (N)
C
C     DWORK   DOUBLE PRECISION array, dimension (3*N)
C
C     Error Indicator
C
C     INFO    INTEGER
C             = 0:  successful exit;
C             < 0:  if INFO = -i, the i-th argument had an illegal
C                   value.
C
C     METHOD
C
C     An estimate is obtained for norm(inv(H)), and the reciprocal of
C     the condition number is computed as
C        RCOND = 1 / ( norm(H) * norm(inv(H)) ).
C
C     REFERENCES
C
C     -
C
C     NUMERICAL ASPECTS
C                                2
C     The algorithm requires 0( N ) operations.
C
C     CONTRIBUTOR
C
C     V. Sima, Katholieke Univ. Leuven, Belgium, June 1998.
C
C     REVISIONS
C
C     V. Sima, May 2020.
C
C     KEYWORDS
C
C     Hessenberg form, matrix algebra.
C
C     ******************************************************************
C
C     .. Parameters ..
      DOUBLE PRECISION   ONE, ZERO
      PARAMETER          ( ONE = 1.0D+0, ZERO = 0.0D+0 )
C     .. Scalar Arguments ..
      CHARACTER          NORM
      INTEGER            INFO, LDH, N
      DOUBLE PRECISION   HNORM, RCOND
C     ..
C     .. Array Arguments ..
      INTEGER            IPIV( * ), IWORK( * )
      DOUBLE PRECISION   DWORK( * ), H( LDH, * )
C     .. Local Scalars ..
      LOGICAL            ONENRM
      CHARACTER          NORMIN
      INTEGER            IX, J, JP, KASE, KASE1
      DOUBLE PRECISION   HINVNM, SCALE, SMLNUM, T
C     .. Local Arrays ..
      INTEGER            ISAVE( 3 )
C     ..
C     .. External Functions ..
      LOGICAL            LSAME
      INTEGER            IDAMAX
      DOUBLE PRECISION   DLAMCH
      EXTERNAL           DLAMCH, IDAMAX, LSAME
C     ..
C     .. External Subroutines ..
      EXTERNAL           DLACN2, DLATRS, DRSCL, XERBLA
C     ..
C     .. Intrinsic Functions ..
      INTRINSIC          ABS, MAX
C     ..
C     .. Executable Statements ..
C
C     Test the input parameters.
C
      INFO = 0
      ONENRM = NORM.EQ.'1' .OR. LSAME( NORM, 'O' )
      IF( .NOT.ONENRM .AND. .NOT.LSAME( NORM, 'I' ) ) THEN
         INFO = -1
      ELSE IF( N.LT.0 ) THEN
         INFO = -2
      ELSE IF( HNORM.LT.ZERO ) THEN
         INFO = -3
      ELSE IF( LDH.LT.MAX( 1, N ) ) THEN
         INFO = -5
      END IF
      IF( INFO.NE.0 ) THEN
         CALL XERBLA( 'MB02TD', -INFO )
         RETURN
      END IF
C
C     Quick return if possible.
C
      RCOND = ZERO
      IF( N.EQ.0 ) THEN
         RCOND = ONE
         RETURN
      ELSE IF( HNORM.EQ.ZERO ) THEN
         RETURN
      END IF
C
      SMLNUM = DLAMCH( 'Safe minimum' )
C
C     Estimate the norm of inv(H).
C
      HINVNM = ZERO
      NORMIN = 'N'
      IF( ONENRM ) THEN
         KASE1 = 1
      ELSE
         KASE1 = 2
      END IF
      KASE = 0
   10 CONTINUE
      CALL DLACN2( N, DWORK( N+1 ), DWORK, IWORK, HINVNM, KASE, ISAVE )
      IF( KASE.NE.0 ) THEN
         IF( KASE.EQ.KASE1 ) THEN
C
C           Multiply by inv(L).
C
            DO 20 J = 1, N - 1
               JP = IPIV( J )
               T = DWORK( JP )
               IF( JP.NE.J ) THEN
                  DWORK( JP ) = DWORK( J )
                  DWORK( J ) = T
               END IF
               DWORK( J+1 ) = DWORK( J+1 ) - T * H( J+1, J )
   20       CONTINUE
C
C           Multiply by inv(U).
C
            CALL DLATRS( 'Upper', 'No transpose', 'Non-unit', NORMIN, N,
     $                   H, LDH, DWORK, SCALE, DWORK( 2*N+1 ), INFO )
         ELSE
C
C           Multiply by inv(U').
C
            CALL DLATRS( 'Upper', 'Transpose', 'Non-unit', NORMIN, N, H,
     $                   LDH, DWORK, SCALE, DWORK( 2*N+1 ), INFO )
C
C           Multiply by inv(L').
C
            DO 30 J = N - 1, 1, -1
               DWORK( J ) = DWORK( J ) - H( J+1, J ) * DWORK( J+1 )
               JP = IPIV( J )
               IF( JP.NE.J ) THEN
                  T = DWORK( JP )
                  DWORK( JP ) = DWORK( J )
                  DWORK( J ) = T
               END IF
   30       CONTINUE
         END IF
C
C        Divide X by 1/SCALE if doing so will not cause overflow.
C
         NORMIN = 'Y'
         IF( SCALE.NE.ONE ) THEN
            IX = IDAMAX( N, DWORK, 1 )
            IF( SCALE.LT.ABS( DWORK( IX ) )*SMLNUM .OR. SCALE.EQ.ZERO
     $        ) GO TO 40
            CALL DRSCL( N, SCALE, DWORK, 1 )
         END IF
         GO TO 10
      END IF
C
C     Compute the estimate of the reciprocal condition number.
C
      IF( HINVNM.NE.ZERO )
     $   RCOND = ( ONE / HINVNM ) / HNORM
C
   40 CONTINUE
      RETURN
C *** Last line of MB02TD ***
      END


c=======================================================================
c
c     subroutine QL0001
c
c     Quadratic solver (vectorized version)
c
c   MINIMIZE        .5*X'*C*X + D'*X
c   SUBJECT TO      A(J)*X  +  B(J)   =  0  ,  J=1,...,ME
c                   A(J)*X  +  B(J)  >=  0  ,  J=ME+1,...,M
c                   XL  <=  X  <=  XU
c
c   HERE C MUST BE AN N BY N SYMMETRIC AND POSITIVE MATRIX, D AN N-DIMENSIONAL
c   VECTOR, A AN M BY N MATRIX AND B AN M-DIMENSIONAL VECTOR. THE ABOVE
c   SITUATION IS INDICATED BY IWAR(1)=1. ALTERNATIVELY, I.E. IF IWAR(1)=0,
c   THE OBJECTIVE FUNCTION MATRIX CAN ALSO BE PROVIDED IN FACTORIZED FORM.
c   IN THIS CASE, C IS AN UPPER TRIANGULAR MATRIX.
c
c   THE SUBROUTINE REORGANIZES SOME DATA SO THAT THE PROBLEM CAN BE SOLVED
c   BY A MODIFICATION OF AN ALGORITHM PROPOSED BY POWELL (1983).
c
c-----------------------------------------------------------------------
      SUBROUTINE ql0001(M,ME,MMAX,N,NMAX,MNN,C,D,A,B,XL,XU,
     &           X,U,IOUT,IFAIL,IPRINT,WAR,LWAR,IWAR,LIWAR,EPS)
c-----------------------------------------------------------------------
C
C   M :        TOTAL NUMBER OF CONSTRAINTS.
C   ME :       NUMBER OF EQUALITY CONSTRAINTS.
C   MMAX :     ROW DIMENSION OF A. MMAX MUST BE AT LEAST ONE AND GREATER
C              THAN M.
C   N :        NUMBER OF VARIABLES.
C   NMAX :     ROW DIMENSION OF C. NMAX MUST BE GREATER OR EQUAL TO N.
C   MNN :      MUST BE EQUAL TO M + N + N.
C   C(NMAX,NMAX): OBJECTIVE FUNCTION MATRIX WHICH SHOULD BE SYMMETRIC AND
C              POSITIVE DEFINITE. IF IWAR(1) = 0, C IS SUPPOSED TO BE THE
C              CHOLESKEY-FACTOR OF ANOTHER MATRIX, I.E. C IS UPPER
C              TRIANGULAR.
C   D(NMAX) :  CONTAINS THE CONSTANT VECTOR OF THE OBJECTIVE FUNCTION.
C   A(MMAX,NMAX): CONTAINS THE DATA MATRIX OF THE LINEAR CONSTRAINTS.
C   B(MMAX) :  CONTAINS THE CONSTANT DATA OF THE LINEAR CONSTRAINTS.
C   XL(N),XU(N): CONTAIN THE LOWER AND UPPER BOUNDS FOR THE VARIABLES.
C   X(N) :     ON RETURN, X CONTAINS THE OPTIMAL SOLUTION VECTOR.
C   U(MNN) :   ON RETURN, U CONTAINS THE LAGRANGE MULTIPLIERS. THE FIRST
C              M POSITIONS ARE RESERVED FOR THE MULTIPLIERS OF THE M
C              LINEAR CONSTRAINTS AND THE SUBSEQUENT ONES FOR THE
C              MULTIPLIERS OF THE LOWER AND UPPER BOUNDS. ON SUCCESSFUL
C              TERMINATION, ALL VALUES OF U WITH RESPECT TO INEQUALITIES
C              AND BOUNDS SHOULD BE GREATER OR EQUAL TO ZERO.
C   IOUT :     INTEGER INDICATING THE DESIRED OUTPUT UNIT NUMBER, I.E.
C              ALL WRITE-STATEMENTS START WITH 'WRITE(IOUT,... '.
C   IFAIL :    SHOWS THE TERMINATION REASON.
C      IFAIL = 0 :   SUCCESSFUL RETURN.
C      IFAIL = 1 :   TOO MANY ITERATIONS (MORE THAN 40*(N+M)).
C      IFAIL = 2 :   ACCURACY INSUFFICIENT TO SATISFY CONVERGENCE
C                    CRITERION.
C      IFAIL = 5 :   LENGTH OF A WORKING ARRAY IS TOO SHORT.
C      IFAIL > 10 :  THE CONSTRAINTS ARE INCONSISTENT.
C   IPRINT :   OUTPUT CONTROL.
C      IPRINT = 0 :  NO OUTPUT OF QL0001.
C      IPRINT > 0 :  BRIEF OUTPUT IN ERROR CASES.
C   WAR(LWAR) : REAL WORKING ARRAY. THE LENGTH LWAR SHOULD BE GRATER THAN
C               3*NMAX*NMAX/2 + 10*NMAX + 2*MMAX.
C   IWAR(LIWAR): INTEGER WORKING ARRAY. THE LENGTH LIWAR SHOULD BE AT
C              LEAST N.
C              IF IWAR(1)=0 INITIALLY, THEN THE CHOLESKY DECOMPOSITION
C              WHICH IS REQUIRED BY THE DUAL ALGORITHM TO GET THE FIRST
C              UNCONSTRAINED MINIMUM OF THE OBJECTIVE FUNCTION, IS
C              PERFORMED INTERNALLY. OTHERWISE, I.E. IF IWAR(1)=1, THEN
C              IT IS ASSUMED THAT THE USER PROVIDES THE INITIAL FAC-
C              TORIZATION BY HIMSELF AND STORES IT IN THE UPPER TRIAN-
C              GULAR PART OF THE ARRAY C.
C
C   EPS DEFINES A GUESS FOR THE UNDERLYING MACHINE PRECISION.
C
c----------------------------------------------------------------------
C
C
      INTEGER NMAX,MMAX,N,MNN,LWAR,LIWAR
      DIMENSION C(NMAX,NMAX),D(NMAX),A(MMAX,NMAX),B(MMAX),
     1      XL(N),XU(N),X(N),U(MNN),WAR(LWAR),IWAR(LIWAR)
      DOUBLE PRECISION C,D,A,B,X,XL,XU,U,WAR,DIAG,ZERO,
     1      EPS,QPEPS,TEN
      INTEGER M,ME,IOUT,IFAIL,IPRINT,IWAR,INW1,INW2,IN,J,LW,MN,I,
     1      IDIAG,INFO,NACT,MAXIT
      LOGICAL LQL
C
C     INTRINSIC FUNCTIONS:  DSQRT
C
C     CONSTANT DATA
C
c#################################################################
c
      if(c(nmax,nmax).eq.0.d0) c(nmax,nmax)=eps
c
c umd
c  This prevents a subsequent more major modification of the Hessian
c  matrix in the important case when a minmax problem (yielding a
c  singular Hessian matrix) is being solved.
c
c#################################################################
c
      LQL=.FALSE.
      IF (IWAR(1).EQ.1) LQL=.TRUE.
      ZERO=0.0D+0
      TEN=1.D+1
      MAXIT=40*(M+N)
      QPEPS=EPS
      INW1=1
      INW2=INW1+MMAX
C
C     PREPARE PROBLEM DATA FOR EXECUTION
C
      IF (M.LE.0) GOTO 20
      IN=INW1
      DO 10 J=1,M
      WAR(IN)=-B(J)
   10 IN=IN+1
   20 LW=3*NMAX*NMAX/2 + 10*NMAX + M
      IF ((INW2+LW).GT.LWAR) GOTO 80
      IF (LIWAR.LT.N) GOTO 81
      IF (MNN.LT.M+N+N) GOTO 82
      MN=M+N
C
C     CALL OF QL0002
C
      CALL QL0002(N,M,ME,MMAX,MN,MNN,NMAX,LQL,A,WAR(INW1),
     1    D,C,XL,XU,X,NACT,IWAR,MAXIT,QPEPS,INFO,DIAG,
     2    WAR(INW2),LW)
C
C     TEST OF MATRIX CORRECTIONS
C
      IFAIL=0
      IF (INFO.EQ.1) GOTO 40
      IF (INFO.EQ.2) GOTO 90
      IDIAG=0
      IF ((DIAG.GT.ZERO).AND.(DIAG.LT.1000.0)) IDIAG=DIAG
      IF ((IPRINT.GT.0).AND.(IDIAG.GT.0))
     1   WRITE(IOUT,1000) IDIAG
      IF (INFO .LT. 0) GOTO  70
C
C     REORDER MULTIPLIER
C
      DO  50 J=1,MNN
   50 U(J)=ZERO
      IN=INW2-1
      IF (NACT.EQ.0) GOTO 30
      DO  60 I=1,NACT
      J=IWAR(I)
      U(J)=WAR(IN+I)
   60 CONTINUE
   30 CONTINUE
      RETURN
C
C     ERROR MESSAGES
C
   70 IFAIL=-INFO+10
      IF ((IPRINT.GT.0).AND.(NACT.GT.0))
     1     WRITE(IOUT,1100) -INFO,(IWAR(I),I=1,NACT)
      RETURN
   80 IFAIL=5
      IF (IPRINT .GT. 0) WRITE(IOUT,1200)
      RETURN
   81 IFAIL=5
      IF (IPRINT .GT. 0) WRITE(IOUT,1210)
      RETURN
   82 IFAIL=5
      IF (IPRINT .GT. 0) WRITE(IOUT,1220)
      RETURN
   40 IFAIL=1
      IF (IPRINT.GT.0) WRITE(IOUT,1300) MAXIT
      RETURN
   90 IFAIL=2
      IF (IPRINT.GT.0) WRITE(IOUT,1400)
      RETURN
C
C     FORMAT-INSTRUCTIONS
C
 1000 FORMAT(/8X,28H***QL: MATRIX G WAS ENLARGED,I3,
     1        20H-TIMES BY UNITMATRIX)
 1100 FORMAT(/8X,18H***QL: CONSTRAINT ,I5,
     1        19H NOT CONSISTENT TO ,/,(10X,10I5))
 1200 FORMAT(/8X,21H***QL: LWAR TOO SMALL)
 1210 FORMAT(/8X,22H***QL: LIWAR TOO SMALL)
 1220 FORMAT(/8X,20H***QL: MNN TOO SMALL)
 1300 FORMAT(/8X,37H***QL: TOO MANY ITERATIONS (MORE THAN,I6,1H))
 1400 FORMAT(/8X,50H***QL: ACCURACY INSUFFICIENT TO ATTAIN CONVERGENCE)
      END
C
      SUBROUTINE QL0002(N,M,MEQ,MMAX,MN,MNN,NMAX,LQL,A,B,GRAD,G,
     1      XL,XU,X,NACT,IACT,MAXIT,VSMALL,INFO,DIAG,W,LW)
C
C**************************************************************************
C
C
C   THIS SUBROUTINE SOLVES THE QUADRATIC PROGRAMMING PROBLEM
C
C       MINIMIZE      GRAD'*X  +  0.5 * X*G*X
C       SUBJECT TO    A(K)*X  =  B(K)   K=1,2,...,MEQ,
C                     A(K)*X >=  B(K)   K=MEQ+1,...,M,
C                     XL  <=  X  <=  XU
C
C   THE QUADRATIC PROGRAMMING METHOD PROCEEDS FROM AN INITIAL CHOLESKY-
C   DECOMPOSITION OF THE OBJECTIVE FUNCTION MATRIX, TO CALCULATE THE
C   UNIQUELY DETERMINED MINIMIZER OF THE UNCONSTRAINED PROBLEM.
C   SUCCESSIVELY ALL VIOLATED CONSTRAINTS ARE ADDED TO A WORKING SET
C   AND A MINIMIZER OF THE OBJECTIVE FUNCTION SUBJECT TO ALL CONSTRAINTS
C   IN THIS WORKING SET IS COMPUTED. IT IS POSSIBLE THAT CONSTRAINTS
C   HAVE TO LEAVE THE WORKING SET.
C
C
C   DESCRIPTION OF PARAMETERS:
C
C     N        : IS THE NUMBER OF VARIABLES.
C     M        : TOTAL NUMBER OF CONSTRAINTS.
C     MEQ      : NUMBER OF EQUALITY CONTRAINTS.
C     MMAX     : ROW DIMENSION OF A, DIMENSION OF B. MMAX MUST BE AT
C                LEAST ONE AND GREATER OR EQUAL TO M.
C     MN       : MUST BE EQUAL M + N.
C     MNN      : MUST BE EQUAL M + N + N.
C     NMAX     : ROW DIEMSION OF G. MUST BE AT LEAST N.
C     LQL      : DETERMINES INITIAL DECOMPOSITION.
C        LQL = .FALSE.  : THE UPPER TRIANGULAR PART OF THE MATRIX G
C                         CONTAINS INITIALLY THE CHOLESKY-FACTOR OF A SUITABLE
C                         DECOMPOSITION.
C        LQL = .TRUE.   : THE INITIAL CHOLESKY-FACTORISATION OF G IS TO BE
C                         PERFORMED BY THE ALGORITHM.
C     A(MMAX,NMAX) : A IS A MATRIX WHOSE COLUMNS ARE THE CONSTRAINTS NORMALS.
C     B(MMAX)  : CONTAINS THE RIGHT HAND SIDES OF THE CONSTRAINTS.
C     GRAD(N)  : CONTAINS THE OBJECTIVE FUNCTION VECTOR GRAD.
C     G(NMAX,N): CONTAINS THE SYMMETRIC OBJECTIVE FUNCTION MATRIX.
C     XL(N), XU(N): CONTAIN THE LOWER AND UPPER BOUNDS FOR X.
C     X(N)     : VECTOR OF VARIABLES.
C     NACT     : FINAL NUMBER OF ACTIVE CONSTRAINTS.
C     IACT(K) (K=1,2,...,NACT): INDICES OF THE FINAL ACTIVE CONSTRAINTS.
C     INFO     : REASON FOR THE RETURN FROM THE SUBROUTINE.
C         INFO = 0 : CALCULATION WAS TERMINATED SUCCESSFULLY.
C         INFO = 1 : MAXIMUM NUMBER OF ITERATIONS ATTAINED.
C         INFO = 2 : ACCURACY IS INSUFFICIENT TO MAINTAIN INCREASING
C                    FUNCTION VALUES.
C         INFO < 0 : THE CONSTRAINT WITH INDEX ABS(INFO) AND THE CON-
C                    STRAINTS WHOSE INDICES ARE IACT(K), K=1,2,...,NACT,
C                    ARE INCONSISTENT.
C     MAXIT    : MAXIMUM NUMBER OF ITERATIONS.
C     VSMALL   : REQUIRED ACCURACY TO BE ACHIEVED (E.G. IN THE ORDER OF THE
C                MACHINE PRECISION FOR SMALL AND WELL-CONDITIONED PROBLEMS).
C     DIAG     : ON RETURN DIAG IS EQUAL TO THE MULTIPLE OF THE UNIT MATRIX
C                THAT WAS ADDED TO G TO ACHIEVE POSITIVE DEFINITENESS.
C     W(LW)    : THE ELEMENTS OF W(.) ARE USED FOR WORKING SPACE. THE LENGTH
C                OF W MUST NOT BE LESS THAN (1.5*NMAX*NMAX + 10*NMAX + M).
C                WHEN INFO = 0 ON RETURN, THE LAGRANGE MULTIPLIERS OF THE
C                FINAL ACTIVE CONSTRAINTS ARE HELD IN W(K), K=1,2,...,NACT.
C   THE VALUES OF N, M, MEQ, MMAX, MN, MNN AND NMAX AND THE ELEMENTS OF
C   A, B, GRAD AND G ARE NOT ALTERED.
C
C   THE FOLLOWING INTEGERS ARE USED TO PARTITION W:
C     THE FIRST N ELEMENTS OF W HOLD LAGRANGE MULTIPLIER ESTIMATES.
C     W(IWZ+I+(N-1)*J) HOLDS THE MATRIX ELEMENT Z(I,J).
C     W(IWR+I+0.5*J*(J-1)) HOLDS THE UPPER TRIANGULAR MATRIX
C       ELEMENT R(I,J). THE SUBSEQUENT N COMPONENTS OF W MAY BE
C       TREATED AS AN EXTRA COLUMN OF R(.,.).
C     W(IWW-N+I) (I=1,2,...,N) ARE USED FOR TEMPORARY STORAGE.
C     W(IWW+I) (I=1,2,...,N) ARE USED FOR TEMPORARY STORAGE.
C     W(IWD+I) (I=1,2,...,N) HOLDS G(I,I) DURING THE CALCULATION.
C     W(IWX+I) (I=1,2,...,N) HOLDS VARIABLES THAT WILL BE USED TO
C       TEST THAT THE ITERATIONS INCREASE THE OBJECTIVE FUNCTION.
C     W(IWA+K) (K=1,2,...,M) USUALLY HOLDS THE RECIPROCAL OF THE
C       LENGTH OF THE K-TH CONSTRAINT, BUT ITS SIGN INDICATES
C       WHETHER THE CONSTRAINT IS ACTIVE.
C
C   REFERENCE: M.J.D. POWELL: ZQPCVX, A FORTRAN SUBROUTINE FOR CONVEX
C              PROGRAMMING, REPORT DAMTP/1983/NA17, UNIVERSITY OF
C              CAMBRIDGE, ENGLAND, 1983.
C
C*************************************************************************
C
      INTEGER MMAX,NMAX,N,LW
      DIMENSION A(MMAX,NMAX),B(MMAX),GRAD(N),G(NMAX,N),X(N),IACT(N),
     1      W(LW),XL(N),XU(N)
      INTEGER M,MEQ,MN,MNN,NACT,IACT,INFO,MAXIT
      DOUBLE PRECISION CVMAX,DIAG,DIAGR,FDIFF,FDIFFA,GA,GB,PARINC,PARNEW
     1      ,RATIO,RES,STEP,SUM,SUMX,SUMY,SUMA,SUMB,SUMC,TEMP,TEMPA,
     2       VSMALL,XMAG,XMAGR,ZERO,ONE,TWO,ONHA,VFACT
      DOUBLE PRECISION A,B,G,GRAD,W,X,XL,XU
C
C   INTRINSIC FUNCTIONS:   DMAX1,DSQRT,DABS,DMIN1
C
      INTEGER IWZ,IWR,IWW,IWD,IWA,IFINC,KFINC,K,I,IA,ID,II,IR,IRA,
     1     IRB,J,NM,IZ,IZA,ITERC,ITREF,JFINC,IFLAG,IWS,IS,K1,IW,KK,IP,
     2     IPP,IL,IU,JU,KFLAG,LFLAG,JFLAG,KDROP,NU,MFLAG,KNEXT,IX,IWX,
     3     IWY,IY,JL
      INTEGER NFLAG,IWWN
      LOGICAL LQL,LOWER
C
C   INITIAL ADDRESSES
C
      IWZ=NMAX
      IWR=IWZ+NMAX*NMAX
      IWW=IWR+(NMAX*(NMAX+3))/2
      IWD=IWW+NMAX
      IWX=IWD+NMAX
      IWA=IWX+NMAX
C
C     SET SOME CONSTANTS.
C
      ZERO=0.D+0
      ONE=1.D+0
      TWO=2.D+0
      ONHA=1.5D+0
      VFACT=1.D+0
C
C     SET SOME PARAMETERS.
C     NUMBER LESS THAN VSMALL ARE ASSUMED TO BE NEGLIGIBLE.
C     THE MULTIPLE OF I THAT IS ADDED TO G IS AT MOST DIAGR TIMES
C       THE LEAST MULTIPLE OF I THAT GIVES POSITIVE DEFINITENESS.
C     X IS RE-INITIALISED IF ITS MAGNITUDE IS REDUCED BY THE
C       FACTOR XMAGR.
C     A CHECK IS MADE FOR AN INCREASE IN F EVERY IFINC ITERATIONS,
C       AFTER KFINC ITERATIONS ARE COMPLETED.
C
      DIAGR=TWO
      XMAGR=1.0D-2
      IFINC=3
      KFINC=MAX0(10,N)
C
C     FIND THE RECIPROCALS OF THE LENGTHS OF THE CONSTRAINT NORMALS.
C     RETURN IF A CONSTRAINT IS INFEASIBLE DUE TO A ZERO NORMAL.
C
      NACT=0
      IF (M .LE. 0) GOTO 45
      DO 40 K=1,M
      SUM=ZERO
      DO 10 I=1,N
   10 SUM=SUM+A(K,I)**2
      IF (SUM .GT. ZERO) GOTO 20
      IF (B(K) .EQ. ZERO) GOTO 30
      INFO=-K
      IF (K .LE. MEQ) GOTO 730
      IF (B(K)) 30,30,730
   20 SUM=ONE/DSQRT(SUM)
   30 IA=IWA+K
   40 W(IA)=SUM
   45 DO 50 K=1,N
      IA=IWA+M+K
   50 W(IA)=ONE
C
C     IF NECESSARY INCREASE THE DIAGONAL ELEMENTS OF G.
C
      IF (.NOT. LQL) GOTO 165
      DIAG=ZERO
      DO 60 I=1,N
      ID=IWD+I
      W(ID)=G(I,I)
      DIAG=DMAX1(DIAG,VSMALL-W(ID))
      IF (I .EQ. N) GOTO 60
      II=I+1
      DO 55 J=II,N
      GA=-DMIN1(W(ID),G(J,J))
      GB=DABS(W(ID)-G(J,J))+DABS(G(I,J))
      IF (GB .GT. ZERO) GA=GA+G(I,J)**2/GB
   55 DIAG=DMAX1(DIAG,GA)
   60 CONTINUE
      IF (DIAG .LE. ZERO) GOTO 90
   70 DIAG=DIAGR*DIAG
      DO 80 I=1,N
      ID=IWD+I
   80 G(I,I)=DIAG+W(ID)
C
C     FORM THE CHOLESKY FACTORISATION OF G. THE TRANSPOSE
C     OF THE FACTOR WILL BE PLACED IN THE R-PARTITION OF W.
C
   90 IR=IWR
      DO 130 J=1,N
      IRA=IWR
      IRB=IR+1
      DO 120 I=1,J
      TEMP=G(I,J)
      IF (I .EQ. 1) GOTO 110
      DO 100 K=IRB,IR
      IRA=IRA+1
  100 TEMP=TEMP-W(K)*W(IRA)
  110 IR=IR+1
      IRA=IRA+1
      IF (I .LT. J) W(IR)=TEMP/W(IRA)
  120 CONTINUE
      IF (TEMP .LT. VSMALL) GOTO 140
  130 W(IR)=DSQRT(TEMP)
      GOTO 170
C
C     INCREASE FURTHER THE DIAGONAL ELEMENT OF G.
C
  140 W(J)=ONE
      SUMX=ONE
      K=J
  150 SUM=ZERO
      IRA=IR-1
      DO 160 I=K,J
      SUM=SUM-W(IRA)*W(I)
  160 IRA=IRA+I
      IR=IR-K
      K=K-1
      W(K)=SUM/W(IR)
      SUMX=SUMX+W(K)**2
      IF (K .GE. 2) GOTO 150
      DIAG=DIAG+VSMALL-TEMP/SUMX
      GOTO 70
C
C     STORE THE CHOLESKY FACTORISATION IN THE R-PARTITION
C     OF W.
C
  165 IR=IWR
      DO 166 I=1,N
      DO 166 J=1,I
      IR=IR+1
  166 W(IR)=G(J,I)
C
C     SET Z THE INVERSE OF THE MATRIX IN R.
C
  170 NM=N-1
      DO 220 I=1,N
      IZ=IWZ+I
      IF (I .EQ. 1) GOTO 190
      DO 180 J=2,I
      W(IZ)=ZERO
  180 IZ=IZ+N
  190 IR=IWR+(I+I*I)/2
      W(IZ)=ONE/W(IR)
      IF (I .EQ. N) GOTO 220
      IZA=IZ
      DO 210 J=I,NM
      IR=IR+I
      SUM=ZERO
      DO 200 K=IZA,IZ,N
      SUM=SUM+W(K)*W(IR)
  200 IR=IR+1
      IZ=IZ+N
  210 W(IZ)=-SUM/W(IR)
  220 CONTINUE
C
C     SET THE INITIAL VALUES OF SOME VARIABLES.
C     ITERC COUNTS THE NUMBER OF ITERATIONS.
C     ITREF IS SET TO ONE WHEN ITERATIVE REFINEMENT IS REQUIRED.
C     JFINC INDICATES WHEN TO TEST FOR AN INCREASE IN F.
C
      ITERC=1
      ITREF=0
      JFINC=-KFINC
C
C     SET X TO ZERO AND SET THE CORRESPONDING RESIDUALS OF THE
C     KUHN-TUCKER CONDITIONS.
C
  230 IFLAG=1
      IWS=IWW-N
      DO 240 I=1,N
      X(I)=ZERO
      IW=IWW+I
      W(IW)=GRAD(I)
      IF (I .GT. NACT) GOTO 240
      W(I)=ZERO
      IS=IWS+I
      K=IACT(I)
      IF (K .LE. M) GOTO 235
      IF (K .GT. MN) GOTO 234
      K1=K-M
      W(IS)=XL(K1)
      GOTO 240
  234 K1=K-MN
      W(IS)=-XU(K1)
      GOTO 240
  235 W(IS)=B(K)
  240 CONTINUE
      XMAG=ZERO
      VFACT=1.D+0
      IF (NACT) 340,340,280
C
C     SET THE RESIDUALS OF THE KUHN-TUCKER CONDITIONS FOR GENERAL X.
C
  250 IFLAG=2
      IWS=IWW-N
      DO 260 I=1,N
      IW=IWW+I
      W(IW)=GRAD(I)
      IF (LQL) GOTO 259
      ID=IWD+I
      W(ID)=ZERO
      DO 251 J=I,N
  251 W(ID)=W(ID)+G(I,J)*X(J)
      DO 252 J=1,I
      ID=IWD+J
  252 W(IW)=W(IW)+G(J,I)*W(ID)
      GOTO 260
  259 DO 261 J=1,N
  261 W(IW)=W(IW)+G(I,J)*X(J)
  260 CONTINUE
      IF (NACT .EQ. 0) GOTO 340
      DO 270 K=1,NACT
      KK=IACT(K)
      IS=IWS+K
      IF (KK .GT. M) GOTO 265
      W(IS)=B(KK)
      DO 264 I=1,N
      IW=IWW+I
      W(IW)=W(IW)-W(K)*A(KK,I)
  264 W(IS)=W(IS)-X(I)*A(KK,I)
      GOTO 270
  265 IF (KK .GT. MN) GOTO 266
      K1=KK-M
      IW=IWW+K1
      W(IW)=W(IW)-W(K)
      W(IS)=XL(K1)-X(K1)
      GOTO 270
  266 K1=KK-MN
      IW=IWW+K1
      W(IW)=W(IW)+W(K)
      W(IS)=-XU(K1)+X(K1)
  270 CONTINUE
C
C     PRE-MULTIPLY THE VECTOR IN THE S-PARTITION OF W BY THE
C     INVERS OF R TRANSPOSE.
C
  280 IR=IWR
      IP=IWW+1
      IPP=IWW+N
      IL=IWS+1
      IU=IWS+NACT
      DO 310 I=IL,IU
      SUM=ZERO
      IF (I .EQ. IL) GOTO 300
      JU=I-1
      DO 290 J=IL,JU
      IR=IR+1
  290 SUM=SUM+W(IR)*W(J)
  300 IR=IR+1
  310 W(I)=(W(I)-SUM)/W(IR)
C
C     SHIFT X TO SATISFY THE ACTIVE CONSTRAINTS AND MAKE THE
C     CORRESPONDING CHANGE TO THE GRADIENT RESIDUALS.
C
      DO 330 I=1,N
      IZ=IWZ+I
      SUM=ZERO
      DO 320 J=IL,IU
      SUM=SUM+W(J)*W(IZ)
  320 IZ=IZ+N
      X(I)=X(I)+SUM
      IF (LQL) GOTO 329
      ID=IWD+I
      W(ID)=ZERO
      DO 321 J=I,N
  321 W(ID)=W(ID)+G(I,J)*SUM
      IW=IWW+I
      DO 322 J=1,I
      ID=IWD+J
  322 W(IW)=W(IW)+G(J,I)*W(ID)
      GOTO 330
  329 DO 331 J=1,N
      IW=IWW+J
  331 W(IW)=W(IW)+SUM*G(I,J)
  330 CONTINUE
C
C     FORM THE SCALAR PRODUCT OF THE CURRENT GRADIENT RESIDUALS
C     WITH EACH COLUMN OF Z.
C
  340 KFLAG=1
      GOTO 930
  350 IF (NACT .EQ. N) GOTO 380
C
C     SHIFT X SO THAT IT SATISFIES THE REMAINING KUHN-TUCKER
C     CONDITIONS.
C
      IL=IWS+NACT+1
      IZA=IWZ+NACT*N
      DO 370 I=1,N
      SUM=ZERO
      IZ=IZA+I
      DO 360 J=IL,IWW
      SUM=SUM+W(IZ)*W(J)
  360 IZ=IZ+N
  370 X(I)=X(I)-SUM
      INFO=0
      IF (NACT .EQ. 0) GOTO 410
C
C     UPDATE THE LAGRANGE MULTIPLIERS.
C
  380 LFLAG=3
      GOTO 740
  390 DO 400 K=1,NACT
      IW=IWW+K
  400 W(K)=W(K)+W(IW)
C
C     REVISE THE VALUES OF XMAG.
C     BRANCH IF ITERATIVE REFINEMENT IS REQUIRED.
C
  410 JFLAG=1
      GOTO 910
  420 IF (IFLAG .EQ. ITREF) GOTO 250
C
C     DELETE A CONSTRAINT IF A LAGRANGE MULTIPLIER OF AN
C     INEQUALITY CONSTRAINT IS NEGATIVE.
C
      KDROP=0
      GOTO 440
  430 KDROP=KDROP+1
      IF (W(KDROP) .GE. ZERO) GOTO 440
      IF (IACT(KDROP) .LE. MEQ) GOTO 440
      NU=NACT
      MFLAG=1
      GOTO 800
  440 IF (KDROP .LT. NACT) GOTO 430
C
C     SEEK THE GREATEAST NORMALISED CONSTRAINT VIOLATION, DISREGARDING
C     ANY THAT MAY BE DUE TO COMPUTER ROUNDING ERRORS.
C
  450 CVMAX=ZERO
      IF (M .LE. 0) GOTO 481
      DO 480 K=1,M
      IA=IWA+K
      IF (W(IA) .LE. ZERO) GOTO 480
      SUM=-B(K)
      DO 460 I=1,N
  460 SUM=SUM+X(I)*A(K,I)
      SUMX=-SUM*W(IA)
      IF (K .LE. MEQ) SUMX=DABS(SUMX)
      IF (SUMX .LE. CVMAX) GOTO 480
      TEMP=DABS(B(K))
      DO 470 I=1,N
  470 TEMP=TEMP+DABS(X(I)*A(K,I))
      TEMPA=TEMP+DABS(SUM)
      IF (TEMPA .LE. TEMP) GOTO 480
      TEMP=TEMP+ONHA*DABS(SUM)
      IF (TEMP .LE. TEMPA) GOTO 480
      CVMAX=SUMX
      RES=SUM
      KNEXT=K
  480 CONTINUE
  481 DO 485 K=1,N
      LOWER=.TRUE.
      IA=IWA+M+K
      IF (W(IA) .LE. ZERO) GOTO 485
      SUM=XL(K)-X(K)
      IF (SUM) 482,485,483
  482 SUM=X(K)-XU(K)
      LOWER=.FALSE.
  483 IF (SUM .LE. CVMAX) GOTO 485
      CVMAX=SUM
      RES=-SUM
      KNEXT=K+M
      IF (LOWER) GOTO 485
      KNEXT=K+MN
  485 CONTINUE
C
C     TEST FOR CONVERGENCE
C
      INFO=0
      IF (CVMAX.ne.CVMAX.OR.CVMAX .LE. VSMALL) GOTO 700
C
C     RETURN IF, DUE TO ROUNDING ERRORS, THE ACTUAL CHANGE IN
C     X MAY NOT INCREASE THE OBJECTIVE FUNCTION
C
      JFINC=JFINC+1
      IF (JFINC .EQ. 0) GOTO 510
      IF (JFINC .NE. IFINC) GOTO 530
      FDIFF=ZERO
      FDIFFA=ZERO
      DO 500 I=1,N
      SUM=TWO*GRAD(I)
      SUMX=DABS(SUM)
      IF (LQL) GOTO 489
      ID=IWD+I
      W(ID)=ZERO
      DO 486 J=I,N
      IX=IWX+J
  486 W(ID)=W(ID)+G(I,J)*(W(IX)+X(J))
      DO 487 J=1,I
      ID=IWD+J
      TEMP=G(J,I)*W(ID)
      SUM=SUM+TEMP
  487 SUMX=SUMX+DABS(TEMP)
      GOTO 495
  489 DO 490 J=1,N
      IX=IWX+J
      TEMP=G(I,J)*(W(IX)+X(J))
      SUM=SUM+TEMP
  490 SUMX=SUMX+DABS(TEMP)
  495 IX=IWX+I
      FDIFF=FDIFF+SUM*(X(I)-W(IX))
  500 FDIFFA=FDIFFA+SUMX*DABS(X(I)-W(IX))
      INFO=2
      SUM=FDIFFA+FDIFF
      IF (SUM .LE. FDIFFA) GOTO 700
      TEMP=FDIFFA+ONHA*FDIFF
      IF (TEMP .LE. SUM) GOTO 700
      JFINC=0
      INFO=0
  510 DO 520 I=1,N
      IX=IWX+I
  520 W(IX)=X(I)
C
C     FORM THE SCALAR PRODUCT OF THE NEW CONSTRAINT NORMAL WITH EACH
C     COLUMN OF Z. PARNEW WILL BECOME THE LAGRANGE MULTIPLIER OF
C     THE NEW CONSTRAINT.
C
  530 ITERC=ITERC+1
      IF (ITERC.LE.MAXIT) GOTO 531
      INFO=1
      GOTO 710
  531 CONTINUE
      IWS=IWR+(NACT+NACT*NACT)/2
      IF (KNEXT .GT. M) GOTO 541
      DO 540 I=1,N
      IW=IWW+I
  540 W(IW)=A(KNEXT,I)
      GOTO 549
  541 DO 542 I=1,N
      IW=IWW+I
  542 W(IW)=ZERO
      K1=KNEXT-M
      IF (K1 .GT. N) GOTO 545
      IW=IWW+K1
      W(IW)=ONE
      IZ=IWZ+K1
      DO 543 I=1,N
      IS=IWS+I
      W(IS)=W(IZ)
  543 IZ=IZ+N
      GOTO 550
  545 K1=KNEXT-MN
      IW=IWW+K1
      W(IW)=-ONE
      IZ=IWZ+K1
      DO 546 I=1,N
      IS=IWS+I
      W(IS)=-W(IZ)
  546 IZ=IZ+N
      GOTO 550
  549 KFLAG=2
      GOTO 930
  550 PARNEW=ZERO
C
C     APPLY GIVENS ROTATIONS TO MAKE THE LAST (N-NACT-2) SCALAR
C     PRODUCTS EQUAL TO ZERO.
C
      IF (NACT .EQ. N) GOTO 570
      NU=N
      NFLAG=1
      GOTO 860
C
C     BRANCH IF THERE IS NO NEED TO DELETE A CONSTRAINT.
C
  560 IS=IWS+NACT
      IF (NACT .EQ. 0) GOTO 640
      SUMA=ZERO
      SUMB=ZERO
      SUMC=ZERO
      IZ=IWZ+NACT*N
      DO 563 I=1,N
      IZ=IZ+1
      IW=IWW+I
      SUMA=SUMA+W(IW)*W(IZ)
      SUMB=SUMB+DABS(W(IW)*W(IZ))
  563 SUMC=SUMC+W(IZ)**2
      TEMP=SUMB+.1D+0*DABS(SUMA)
      TEMPA=SUMB+.2D+0*DABS(SUMA)
      IF (TEMP .LE. SUMB) GOTO 570
      IF (TEMPA .LE. TEMP) GOTO 570
      IF (SUMB .GT. VSMALL) GOTO 5
      GOTO 570
    5 SUMC=DSQRT(SUMC)
      IA=IWA+KNEXT
      IF (KNEXT .LE. M) SUMC=SUMC/W(IA)
      TEMP=SUMC+.1D+0*DABS(SUMA)
      TEMPA=SUMC+.2D+0*DABS(SUMA)
      IF (TEMP .LE. SUMC) GOTO 567
      IF (TEMPA .LE. TEMP) GOTO 567
      GOTO 640
C
C     CALCULATE THE MULTIPLIERS FOR THE NEW CONSTRAINT NORMAL
C     EXPRESSED IN TERMS OF THE ACTIVE CONSTRAINT NORMALS.
C     THEN WORK OUT WHICH CONTRAINT TO DROP.
C
  567 LFLAG=4
      GOTO 740
  570 LFLAG=1
      GOTO 740
C
C     COMPLETE THE TEST FOR LINEARLY DEPENDENT CONSTRAINTS.
C
  571 IF (KNEXT .GT. M) GOTO 574
      DO 573 I=1,N
      SUMA=A(KNEXT,I)
      SUMB=DABS(SUMA)
      IF (NACT.EQ.0) GOTO 581
      DO 572 K=1,NACT
      KK=IACT(K)
      IF (KK.LE.M) GOTO 568
      KK=KK-M
      TEMP=ZERO
      IF (KK.EQ.I) TEMP=W(IWW+KK)
      KK=KK-N
      IF (KK.EQ.I) TEMP=-W(IWW+KK)
      GOTO 569
  568 CONTINUE
      IW=IWW+K
      TEMP=W(IW)*A(KK,I)
  569 CONTINUE
      SUMA=SUMA-TEMP
  572 SUMB=SUMB+DABS(TEMP)
  581 IF (SUMA .LE. VSMALL) GOTO 573
      TEMP=SUMB+.1D+0*DABS(SUMA)
      TEMPA=SUMB+.2D+0*DABS(SUMA)
      IF (TEMP .LE. SUMB) GOTO 573
      IF (TEMPA .LE. TEMP) GOTO 573
      GOTO 630
  573 CONTINUE
      LFLAG=1
      GOTO 775
  574 K1=KNEXT-M
      IF (K1 .GT. N) K1=K1-N
      DO 578 I=1,N
      SUMA=ZERO
      IF (I .NE. K1) GOTO 575
      SUMA=ONE
      IF (KNEXT .GT. MN) SUMA=-ONE
  575 SUMB=DABS(SUMA)
      IF (NACT.EQ.0) GOTO 582
      DO 577 K=1,NACT
      KK=IACT(K)
      IF (KK .LE. M) GOTO 579
      KK=KK-M
      TEMP=ZERO
      IF (KK.EQ.I) TEMP=W(IWW+KK)
      KK=KK-N
      IF (KK.EQ.I) TEMP=-W(IWW+KK)
      GOTO 576
  579 IW=IWW+K
      TEMP=W(IW)*A(KK,I)
  576 SUMA=SUMA-TEMP
  577 SUMB=SUMB+DABS(TEMP)
  582 TEMP=SUMB+.1D+0*DABS(SUMA)
      TEMPA=SUMB+.2D+0*DABS(SUMA)
      IF (TEMP .LE. SUMB) GOTO 578
      IF (TEMPA .LE. TEMP) GOTO 578
      GOTO 630
  578 CONTINUE
      LFLAG=1
      GOTO 775
C
C     BRANCH IF THE CONTRAINTS ARE INCONSISTENT.
C
  580 INFO=-KNEXT
      IF (KDROP .EQ. 0) GOTO 700
      PARINC=RATIO
      PARNEW=PARINC
C
C     REVISE THE LAGRANGE MULTIPLIERS OF THE ACTIVE CONSTRAINTS.
C
  590 IF (NACT.EQ.0) GOTO 601
      DO 600 K=1,NACT
      IW=IWW+K
      W(K)=W(K)-PARINC*W(IW)
      IF (IACT(K) .GT. MEQ) W(K)=DMAX1(ZERO,W(K))
  600 CONTINUE
  601 IF (KDROP .EQ. 0) GOTO 680
C
C     DELETE THE CONSTRAINT TO BE DROPPED.
C     SHIFT THE VECTOR OF SCALAR PRODUCTS.
C     THEN, IF APPROPRIATE, MAKE ONE MORE SCALAR PRODUCT ZERO.
C
      NU=NACT+1
      MFLAG=2
      GOTO 800
  610 IWS=IWS-NACT-1
      NU=MIN0(N,NU)
      DO 620 I=1,NU
      IS=IWS+I
      J=IS+NACT
  620 W(IS)=W(J+1)
      NFLAG=2
      GOTO 860
C
C     CALCULATE THE STEP TO THE VIOLATED CONSTRAINT.
C
  630 IS=IWS+NACT
  640 SUMY=W(IS+1)
      STEP=-RES/SUMY
      PARINC=STEP/SUMY
      IF (NACT .EQ. 0) GOTO 660
C
C     CALCULATE THE CHANGES TO THE LAGRANGE MULTIPLIERS, AND REDUCE
C     THE STEP ALONG THE NEW SEARCH DIRECTION IF NECESSARY.
C
      LFLAG=2
      GOTO 740
  650 IF (KDROP .EQ. 0) GOTO 660
      TEMP=ONE-RATIO/PARINC
      IF (TEMP .LE. ZERO) KDROP=0
      IF (KDROP .EQ. 0) GOTO 660
      STEP=RATIO*SUMY
      PARINC=RATIO
      RES=TEMP*RES
C
C     UPDATE X AND THE LAGRANGE MULTIPIERS.
C     DROP A CONSTRAINT IF THE FULL STEP IS NOT TAKEN.
C
  660 IWY=IWZ+NACT*N
      DO 670 I=1,N
      IY=IWY+I
  670 X(I)=X(I)+STEP*W(IY)
      PARNEW=PARNEW+PARINC
      IF (NACT .GE. 1) GOTO 590
C
C     ADD THE NEW CONSTRAINT TO THE ACTIVE SET.
C
  680 NACT=NACT+1
      W(NACT)=PARNEW
      IACT(NACT)=KNEXT
      IA=IWA+KNEXT
      IF (KNEXT .GT. MN) IA=IA-N
      W(IA)=-W(IA)
C
C     ESTIMATE THE MAGNITUDE OF X. THEN BEGIN A NEW ITERATION,
C     RE-INITILISING X IF THIS MAGNITUDE IS SMALL.
C
      JFLAG=2
      GOTO 910
  690 IF (SUM .LT. (XMAGR*XMAG)) GOTO 230
      IF (ITREF) 450,450,250
C
C     INITIATE ITERATIVE REFINEMENT IF IT HAS NOT YET BEEN USED,
C     OR RETURN AFTER RESTORING THE DIAGONAL ELEMENTS OF G.
C
  700 IF (ITERC .EQ. 0) GOTO 710
      ITREF=ITREF+1
      JFINC=-1
      IF (ITREF .EQ. 1) GOTO 250
  710 IF (.NOT. LQL) RETURN
      DO 720 I=1,N
      ID=IWD+I
  720 G(I,I)=W(ID)
  730 RETURN
C
C
C     THE REMAINIG INSTRUCTIONS ARE USED AS SUBROUTINES.
C
C********************************************************************
C
C     CALCULATE THE LAGRANGE MULTIPLIERS BY PRE-MULTIPLYING THE
C     VECTOR IN THE S-PARTITION OF W BY THE INVERSE OF R.
C
  740 IR=IWR+(NACT+NACT*NACT)/2
      I=NACT
      SUM=ZERO
      GOTO 770
  750 IRA=IR-1
      SUM=ZERO
      IF (NACT.EQ.0) GOTO 761
      DO 760 J=I,NACT
      IW=IWW+J
      SUM=SUM+W(IRA)*W(IW)
  760 IRA=IRA+J
  761 IR=IR-I
      I=I-1
  770 IW=IWW+I
      IS=IWS+I
      W(IW)=(W(IS)-SUM)/W(IR)
      IF (I .GT. 1) GOTO 750
      IF (LFLAG .EQ. 3) GOTO 390
      IF (LFLAG .EQ. 4) GOTO 571
C
C     CALCULATE THE NEXT CONSTRAINT TO DROP.
C
  775 IP=IWW+1
      IPP=IWW+NACT
      KDROP=0
      IF (NACT.EQ.0) GOTO 791
      DO 790 K=1,NACT
      IF (IACT(K) .LE. MEQ) GOTO 790
      IW=IWW+K
      IF ((RES*W(IW)) .GE. ZERO) GOTO 790
      TEMP=W(K)/W(IW)
      IF (KDROP .EQ. 0) GOTO 780
      IF (DABS(TEMP) .GE. DABS(RATIO)) GOTO 790
  780 KDROP=K
      RATIO=TEMP
  790 CONTINUE
  791 GOTO (580,650), LFLAG
C
C********************************************************************
C
C     DROP THE CONSTRAINT IN POSITION KDROP IN THE ACTIVE SET.
C
  800 IA=IWA+IACT(KDROP)
      IF (IACT(KDROP) .GT. MN) IA=IA-N
      W(IA)=-W(IA)
      IF (KDROP .EQ. NACT) GOTO 850
C
C     SET SOME INDICES AND CALCULATE THE ELEMENTS OF THE NEXT
C     GIVENS ROTATION.
C
      IZ=IWZ+KDROP*N
      IR=IWR+(KDROP+KDROP*KDROP)/2
  810 IRA=IR
      IR=IR+KDROP+1
      TEMP=DMAX1(DABS(W(IR-1)),DABS(W(IR)))
      SUM=TEMP*DSQRT((W(IR-1)/TEMP)**2+(W(IR)/TEMP)**2)
      GA=W(IR-1)/SUM
      GB=W(IR)/SUM
C
C     EXCHANGE THE COLUMNS OF R.
C
      DO 820 I=1,KDROP
      IRA=IRA+1
      J=IRA-KDROP
      TEMP=W(IRA)
      W(IRA)=W(J)
  820 W(J)=TEMP
      W(IR)=ZERO
C
C     APPLY THE ROTATION TO THE ROWS OF R.
C
      W(J)=SUM
      KDROP=KDROP+1
      DO 830 I=KDROP,NU
      TEMP=GA*W(IRA)+GB*W(IRA+1)
      W(IRA+1)=GA*W(IRA+1)-GB*W(IRA)
      W(IRA)=TEMP
  830 IRA=IRA+I
C
C     APPLY THE ROTATION TO THE COLUMNS OF Z.
C
      DO 840 I=1,N
      IZ=IZ+1
      J=IZ-N
      TEMP=GA*W(J)+GB*W(IZ)
      W(IZ)=GA*W(IZ)-GB*W(J)
  840 W(J)=TEMP
C
C     REVISE IACT AND THE LAGRANGE MULTIPLIERS.
C
      IACT(KDROP-1)=IACT(KDROP)
      W(KDROP-1)=W(KDROP)
      IF (KDROP .LT. NACT) GOTO 810
  850 NACT=NACT-1
      GOTO (250,610), MFLAG
C
C********************************************************************
C
C     APPLY GIVENS ROTATION TO REDUCE SOME OF THE SCALAR
C     PRODUCTS IN THE S-PARTITION OF W TO ZERO.
C
  860 IZ=IWZ+NU*N
  870 IZ=IZ-N
  880 IS=IWS+NU
      NU=NU-1
      IF (NU .EQ. NACT) GOTO 900
      IF (W(IS) .EQ. ZERO) GOTO 870
      TEMP=DMAX1(DABS(W(IS-1)),DABS(W(IS)))
      SUM=TEMP*DSQRT((W(IS-1)/TEMP)**2+(W(IS)/TEMP)**2)
      GA=W(IS-1)/SUM
      GB=W(IS)/SUM
      W(IS-1)=SUM
      DO 890 I=1,N
      K=IZ+N
      TEMP=GA*W(IZ)+GB*W(K)
      W(K)=GA*W(K)-GB*W(IZ)
      W(IZ)=TEMP
  890 IZ=IZ-1
      GOTO 880
  900 GOTO (560,630), NFLAG
C
C********************************************************************
C
C     CALCULATE THE MAGNITUDE OF X AN REVISE XMAG.
C
  910 SUM=ZERO
      DO 920 I=1,N
      SUM=SUM+DABS(X(I))*VFACT*(DABS(GRAD(I))+DABS(G(I,I)*X(I)))
      IF (LQL) GOTO 920
      IF (SUM .LT. 1.D-30) GOTO 920
      VFACT=1.D-10*VFACT
      SUM=1.D-10*SUM
      XMAG=1.D-10*XMAG
  920 CONTINUE
  925 XMAG=DMAX1(XMAG,SUM)
      GOTO (420,690), JFLAG
C
C********************************************************************
C
C     PRE-MULTIPLY THE VECTOR IN THE W-PARTITION OF W BY Z TRANSPOSE.
C
  930 JL=IWW+1
      IZ=IWZ
      DO 940 I=1,N
      IS=IWS+I
      W(IS)=ZERO
      IWWN=IWW+N
      DO 940 J=JL,IWWN
      IZ=IZ+1
  940 W(IS)=W(IS)+W(IZ)*W(J)
      GOTO (350,550), KFLAG
      RETURN
      END


!/*
! * EvolutionOperator_RWA.f90
! *
! *  Created on: 8 Oct 2017
! *      Author: german
! */

!!$SUBROUTINE EVOLUTIONOPERATOR_RWA(D,U_MODES,ENERGIES,&
!!$     & FIELD,T1,T2,P,INFO)
!!$
!!$  USE FLOQUET
!!$  USE TYPES
!!$  USE SUBINTERFACE_LAPACK
!!$
!!$
!!$  IMPLICIT NONE
!!$  INTEGER,                                INTENT(IN)    :: D
!!$  COMPLEX*16,       DIMENSION(D,D),       INTENT(IN)    :: U_MODES
!!$  DOUBLE PRECISION, DIMENSION(D),         INTENT(IN)    :: ENERGIES
!!$  TYPE(MODE),       DIMENSION(MODES_NUM), INTENT(IN)    :: FIELD  ! FIELDS PROPERTIES: FREQUENCY, AMPLITUDES AND PHASES
!!$  DOUBLE PRECISION,                       INTENT(IN)    :: T1,T2
!!$  COMPLEX*16,       DIMENSION(D,D),       INTENT(OUT)   :: P
!!$  INTEGER,                                INTENT(INOUT) :: INFO
!!$
!!$  COMPLEX*16,       DIMENSION(D,D) :: IDENTITY
!!$
!!$  INTEGER m
!!$
!!$  P = 0.0
!!$  DO m=1,D
!!$     P(m,m) = EXP(-DCMPLX(0.0,1.0)*ENERGIES(m)*(T2-T1))
!!$  END DO
!!$
!!$  P = MATMUL(U_MODES,MATMUL(P,TRANSPOSE(CONJG(U_MODES))))
!!$
!!$END SUBROUTINE EVOLUTIONOPERATOR_RWA

SUBROUTINE EVOLUTIONOPERATOR_RWA_qubit(D,FIELD,T1,T2,P_BARE,INFO)

  USE FLOQUET
  USE TYPES
  USE SUBINTERFACE_LAPACK

  IMPLICIT NONE
  INTEGER,                         INTENT(IN)     :: D
  INTEGER,                         INTENT(INOUT)  :: INFO
  DOUBLE PRECISION,                INTENT(IN)     :: T1,T2
  COMPLEX*16, DIMENSION(D,D),      INTENT(OUT)     :: P_BARE
  TYPE(MODE),       DIMENSION(MODES_NUM),INTENT(OUT) :: FIELD

  COMPLEX*16,       DIMENSION(D,D) :: IDENTITY
  COMPLEX*16, DIMENSION(:,:), ALLOCATABLE :: U_Rot_1,U_Rot_2,U_RWA_1,U_RWA_2,U_TimeEvol

  INTEGER m
  DOUBLE PRECISION :: delta_1,delta_2,omega_1,omega_2,phi_1,phi_2,cos_2theta_1,cos_2theta_2,E_d_1,E_d_2,OMEGA_R_1,OMEGA_R_2

  IDENTITY = 0.0

  DO m=1,D
   IDENTITY(m,m) = 1.0
  END DO

  omega_1      = FIELD(2)%OMEGA
  phi_1        = FIELD(2)%phi_x
  OMEGA_R_1    = FIELD(2)%BX
  delta_1      = 1.0 - FIELD(2)%OMEGA
  E_d_1        = SQRT(delta_1**2+(OMEGA_R_1/2.0)**2)
  cos_2theta_1 = -delta_1/(E_d_1)

  omega_2      = FIELD(3)%OMEGA
  phi_2        = FIELD(3)%phi_z
  OMEGA_R_2    = FIELD(3)%BZ*sqrt(1-cos_2theta_1)
  delta_2      = E_D_1 - FIELD(3)%OMEGA
  E_d_2        = SQRT(delta_2**2+(OMEGA_R_2/2.0)**2)
  cos_2theta_2 = -delta_2/(E_d_2)

!  write(*,*) omega_2,sqrt((1+cos_2theta_1)/2.0)*sqrt((1+cos_2theta_2)/2.0), sqrt((1+cos_2theta_1)/2.0)*sqrt((1-cos_2theta_2)/2.0), &
!                &    sqrt((1-cos_2theta_1)/2.0)*sqrt((1-cos_2theta_2)/2.0),-sqrt((1-cos_2theta_1)/2.0)*sqrt((1+cos_2theta_2)/2.0), &
!                &    sqrt((1-cos_2theta_1)/2.0)*sqrt((1+cos_2theta_2)/2.0), sqrt((1-cos_2theta_1)/2.0)*sqrt((1-cos_2theta_2)/2.0), &
!                &   -sqrt((1+cos_2theta_1)/2.0)*sqrt((1-cos_2theta_2)/2.0), sqrt((1+cos_2theta_1)/2.0)*sqrt((1+cos_2theta_2)/2.0)

  ALLOCATE(U_ROT_1(D,D))
  ALLOCATE(U_RWA_1(D,D))

  ALLOCATE(U_ROT_2(D,D))
  ALLOCATE(U_RWA_2(D,D))

  ALLOCATE(U_TimeEvol(D,D))

  U_ROT_1 = 0.0
  U_ROT_2 = 0.0
  U_RWA_1 = 0.0
  U_RWA_2 = 0.0

  U_ROT_1(1,1) = exp(-DCMPLX(0.0,1.0)*omega_1*t2/2)
  U_ROT_1(2,2) = exp(DCMPLX(0.0,1.0)*omega_1*t2/2)

  U_ROT_2(1,1) = exp(-DCMPLX(0.0,1.0)*omega_2*t2/2)
  U_ROT_2(2,2) = exp(DCMPLX(0.0,1.0)*omega_2*t2/2)

  U_RWA_1(1,1)  = ( exp(-0.5*DCMPLX(0.0,1.0)*phi_1))*sqrt(0.5*(1-cos_2theta_1))
  U_RWA_1(1,2)  = ( exp(-0.5*DCMPLX(0.0,1.0)*phi_1))*sqrt(0.5*(1+cos_2theta_1))
  U_RWA_1(2,1)  = ( exp( 0.5*DCMPLX(0.0,1.0)*phi_1))*sqrt(0.5*(1+cos_2theta_1))
  U_RWA_1(2,2)  = (-exp( 0.5*DCMPLX(0.0,1.0)*phi_1))*sqrt(0.5*(1-cos_2theta_1))

  U_RWA_2(1,1)  = ( exp(-0.5*DCMPLX(0.0,1.0)*phi_2))*sqrt(0.5*(1-cos_2theta_2))
  U_RWA_2(1,2)  = ( exp(-0.5*DCMPLX(0.0,1.0)*phi_2))*sqrt(0.5*(1+cos_2theta_2))
  U_RWA_2(2,1)  = ( exp( 0.5*DCMPLX(0.0,1.0)*phi_2))*sqrt(0.5*(1+cos_2theta_2))
  U_RWA_2(2,2)  = (-exp( 0.5*DCMPLX(0.0,1.0)*phi_2))*sqrt(0.5*(1-cos_2theta_2))

  !P_BARE = MATMUL(U_ROT_1,U_RWA_1)

  !P_BARE = MATMUL(U_ROT_1,MATMUL(U_RWA_1,MATMUL(U_ROT_2,U_RWA_2)))

!write(*,*) abs(U_RWA_2(1,1)),cos_2theta_2,-delta_2,E_d_2
  ! Time evolution in the dressed basis of the double rotating frame
  U_TimeEvol = 0.0
  U_TimeEvol(1,1) = exp(-DCMPLX(0.0,1.0)*E_d_2*(t2-t1)/2.0)
  U_TimeEvol(2,2) = exp( DCMPLX(0.0,1.0)*E_d_2*(t2-t1)/2.0)

  ! Time evolution in the dressed basis
  U_TimeEvol = MATMUL(MATMUL(U_RWA_2,U_TimeEvol),TRANSPOSE(CONJG(U_RWA_2)))
  U_TimeEvol = MATMUL(U_ROT_2,U_TimeEvol)
  U_ROT_2(1,1) = exp(-DCMPLX(0.0,1.0)*omega_2*t1/2)
  U_ROT_2(2,2) = exp( DCMPLX(0.0,1.0)*omega_2*t1/2)
  U_TimeEvol = MATMUL(U_TimeEvol,transpose(conjg(U_ROT_2)))

  ! Time evolution in the bare basis
  U_TimeEvol = MATMUL(MATMUL(U_RWA_1,U_TimeEvol),TRANSPOSE(CONJG(U_RWA_1)))
  U_TimeEvol = MATMUL(U_ROT_1,U_TimeEvol)
  U_ROT_1(1,1) = exp(-DCMPLX(0.0,1.0)*omega_2*t1/2)
  U_ROT_1(2,2) = exp( DCMPLX(0.0,1.0)*omega_2*t1/2)
  U_TimeEvol = MATMUL(U_TimeEvol,transpose(conjg(U_ROT_1)))

  P_BARE = ABS(U_TimeEvol)**2

END SUBROUTINE EVOLUTIONOPERATOR_RWA_qubit


SUBROUTINE clawpack5_src2(meqn,mbc,mx,my,xlower,ylower, &
     dx,dy,q,maux,aux,t,dt)
  IMPLICIT NONE

  INTEGER meqn,mbc,mx,my,maux
  DOUBLE PRECISION xlower,ylower,dx,dy,t,dt
  DOUBLE PRECISION    q(meqn,1-mbc:mx+mbc, 1-mbc:my+mbc)
  DOUBLE PRECISION  aux(maux,1-mbc:mx+mbc, 1-mbc:my+mbc)

  DOUBLE PRECISION qstar(4)
  DOUBLE PRECISION gamma,gamma1
  COMMON /cparam/  gamma,gamma1

  INTEGER i,j,ndim
  DOUBLE PRECISION dt2,press,rad,rho,u,v

  !! # 2-stage Runge-Kutta method

  dt2    = dt/2.d0
  press  = 0.d0
  ndim   = 2

  DO  i = 1,mx
     DO j = 1,my
        rad      = aux(1,i,j)
        rho      = q(1,i,j)
        u        = q(2,i,j)/q(1,i,j)
        v        = q(3,i,j)/q(1,i,j)
        press    = gamma1*(q(4,i,j) - 0.5d0*rho*(u**2 + v**2))

        IF (rad.EQ.0.d0) WRITE(6,*) 'rad = 0 in src2'
        IF (rho.EQ.0.d0) WRITE(6,*) 'rho = 0 in q'

        qstar(1) = q(1,i,j) - dt2*(ndim-1)/rad * q(3,i,j)
        qstar(2) = q(2,i,j) - dt2*(ndim-1)/rad * (rho*u*v)
        qstar(3) = q(3,i,j) - dt2*(ndim-1)/rad * (rho*v*v)
        qstar(4) = q(4,i,j) - dt2*(ndim-1)/rad * v*(q(4,i,j) + press)

        !! # second stage

        rho      = qstar(1)
        u        = qstar(2)/qstar(1)
        v        = qstar(3)/qstar(1)
        press    = gamma1*(qstar(4) - 0.5d0*rho*(u**2 + v**2))
        IF (rho.EQ.0.d0) WRITE(6,*) 'rho = 0 in qstar'

        q(1,i,j) = q(1,i,j) - dt*(ndim-1)/rad * qstar(3)
        q(2,i,j) = q(2,i,j) - dt*(ndim-1)/rad * (rho*u*v)
        q(3,i,j) = q(3,i,j) - dt*(ndim-1)/rad * (rho*v*v)
        q(4,i,j) = q(4,i,j) - dt*(ndim-1)/rad * v*(qstar(4) + press)
     ENDDO
  ENDDO

  RETURN
END SUBROUTINE clawpack5_src2


MODULE diag
  IMPLICIT NONE
  !
  ! Numerical mixing diagnostics are computed in Subroutine correlation_diag below.
  !
  ! Filament preservation diagnostic is computed in Subroutine filament_diag below.
  !
  ! It is assumed that the data is in one-dimensional arrays of length K, however,
  ! the code can easily be changed to acomodate other data structures.
  !
  ! dA(j) is the spherical area of grid cell j
  ! f1 is the mixing ratio of tracer 1
  ! f2 is the mixing ratio of tracer 2
  !

CONTAINS
  !
  ! compute correlation diagnostics
  !

  SUBROUTINE correlation_diag(f1,f2,K,dA,diagout)
    IMPLICIT NONE
    INTEGER, INTENT(IN) :: K
    REAL, DIMENSION(K)      , INTENT(IN) :: f1,f2,dA
    REAL :: sumA, diagout(3)
    !
    ! local workspace
    !
    REAL     :: root, tol,q1,q2,real_mixing,overshooting,range_pres_unmixing,c
    INTEGER  :: j
    REAL     :: q1_min,q1_max,q2_min,q2_max
    REAL     :: total_area, sqrt_arg

    q1_min = 0.1
    q1_max = 1.0

!! Changed so that end points of line segement are (q1_min,q2_min) and (q1_max,q2_max)
!! Note that "q2_min > q2_max".
    q2_min = corr_fct(q1_min)
    q2_max = corr_fct(q1_max)

    real_mixing          = 0.0
    overshooting         = 0.0
    range_pres_unmixing  = 0.0

    total_area = 0.0

    sumA = 0
    DO j = 1,K
       total_area = total_area+dA(j)

       q1 = f1(j)
       q2 = f2(j)

       !! Check to make sure we are not in the (very unlikely) situation where the argument
       !! to the sqrt (below) is negative.  This will happen if we are in a region which
       !! the cubic has three real roots, and so we'd have to be careful about which one we pick.
       sqrt_arg = (-DBLE(1687296) + DBLE(12168000)*q2 &
            -DBLE(29250000)*q2**2+DBLE(23437500)*q2**3+DBLE(29648025)*q1**2)
       IF (sqrt_arg < 0) THEN
          WRITE(6,*) 'Warning : (xk,yk) data is in region where there are three possible ', &
               ' real roots to the closest point problem; ', j
          continue
!!          STOP
       ENDIF

       c = (DBLE(65340)*q1+12.*SQRT(-DBLE(1687296)+DBLE(12168000)*q2 &
            -DBLE(29250000)*q2**2+DBLE(23437500)*q2**3+DBLE(29648025)*q1**2))**(1./3.)
       c=c/(DBLE(60))

       root = c-(-(DBLE(13)/DBLE(75))+(DBLE(5)/DBLE(12))*q2)/c
       root = MAX(0.1,root)
       root = MIN(1.0,root)

       !! Also fixed bug here : call to line_fct passed in q2 instead of q1 as argument.
       IF (q2 < corr_fct(q1).AND.q2 > line_fct(q1,q1_min,q1_max,q2_min,q2_max)) THEN
          !
          ! `real' mixing
          !
          real_mixing = real_mixing + dist_fct(root,q1,q2)*dA(j)
       ELSE IF (q1 < q1_max.AND.q1 > q1_min.AND.q2 < q2_min.AND.q2 > q2_max) THEN
          !! Note that "q2_min > q2_max", so this 'if' branch had to be modifed

          !
          ! range-preserving unmixing
          !
          range_pres_unmixing = range_pres_unmixing+dist_fct(root,q1,q2)*dA(j)
       ELSE
          !
          ! overshooting
          !
          overshooting = overshooting + dist_fct(root,q1,q2)*dA(j)
       END IF
       sumA = sumA + dist_fct(root,q1,q2)*dA(j)
    END DO
    WRITE(*,*) "========================================================================"
    WRITE(*,*) " "
    WRITE(*,*) "mixing diagnostics"
    WRITE(*,*) " "
    WRITE(*,*) "------------------------------------------------------------------------"
    WRITE(*,*) " "
    WRITE(*,*) "real_mixing ",real_mixing/total_area
    WRITE(*,*) "range_pres_unmixing ",range_pres_unmixing/total_area
    WRITE(*,*) "overshooting     ",overshooting/total_area
    WRITE(*,*) " "
    WRITE(*,*) "real_mixing (frac) ", (real_mixing/sumA)
    WRITE(*,*) "range_pres_mixing (frac) ", (range_pres_unmixing/sumA)
    WRITE(*,*) "over_shooting (frac) ", (overshooting/sumA)
    WRITE(*,*) "sumA " , sumA
    WRITE(*,*) "========================================================================"

    diagout(1) = real_mixing/total_area
    diagout(2) = range_pres_unmixing/total_area
    diagout(3) = overshooting/total_area
  END SUBROUTINE correlation_diag

  !
  ! correlation function
  !
  REAL FUNCTION corr_fct(x)
    IMPLICIT NONE
    REAL , INTENT(IN)  :: x
    corr_fct = -0.8*x**2+0.9
  END FUNCTION corr_fct
  !
  ! Eucledian distance function
  !
  REAL FUNCTION dist_fct(x,x0,y0)
    IMPLICIT NONE
    REAL , INTENT(IN)  :: x,x0,y0
    dist_fct = SQRT((x-x0)*(x-x0)/(0.9**2)+(corr_fct(x)-y0)*(corr_fct(x)-y0)/(0.792**2))
  END FUNCTION dist_fct
  !
  ! straight line line function
  !
  REAL FUNCTION line_fct(x,xmin,xmax,ymin,ymax)
    IMPLICIT NONE
    REAL , INTENT(IN)  :: x,xmin,xmax,ymin,ymax
    REAL  :: a,b
    !
    ! line: y=a*x+b
    !
    a = (ymax-ymin)/(xmax-xmin)
    b = ymin-xmin*a
    line_fct = a*x+b
  END FUNCTION line_fct

  !
  ! linit = .TRUE. if t=0 else .FALSE.
  !
  SUBROUTINE filament_diag(K,f1,dA,fila_t0,linit)
    IMPLICIT NONE
    INTEGER, INTENT(IN)                    :: K
    REAL   , DIMENSION(K)  , INTENT(IN)    :: f1, dA
    REAL   , DIMENSION(100), INTENT(INOUT) :: fila_t0
    LOGICAL                , INTENT(IN)    :: linit
    !
    ! local workspace
    !
    REAL    :: threshold,tiny
    REAL    :: out, a
    INTEGER :: j,jk,jlevels

    tiny=1.0E-12
    !
    OPEN (unit = 31, file='filament.dat',status='replace')
    jlevels = 18
    DO jk = 0,jlevels
       threshold = 0.1d0 + jk/20.d0
       out = 0.0
       DO j = 1,K
          IF (f1(j) .GE. threshold-tiny) THEN
             out = out + dA(j)
          END IF
       END DO
       IF (linit) THEN
          fila_t0(jk) = out
       ELSE
          IF (fila_t0(jk) .EQ. 0) THEN
             a = 0.d0
          ELSE
             a = 100*out/fila_t0(jk)
          ENDIF
          WRITE(31,100) threshold,a
       END IF
    END DO
    CLOSE(31)
100 FORMAT(F5.2,F8.2)
  END SUBROUTINE filament_diag
END MODULE diag


      SUBROUTINE TVECT(E, X, L, D, U, N, W)
!      ________________________________________________________
!     |                                                        |
!     |     COMPUTE EIGENVECTOR CORRESPONDING TO GIVEN REAL    |
!     |        EIGENVALUE FOR A REAL TRIDIAGONAL MATRIX        |
!     |                                                        |
!     |    INPUT:                                              |
!     |                                                        |
!     |         E     --EIGENVALUE                             |
!     |                                                        |
!     |         L     --SUBDIAGONAL                            |
!     |                                                        |
!     |         D     --DIAGONAL                               |
!     |                                                        |
!     |         U     --SUPERDIAGONAL                          |
!     |                                                        |
!     |         N     --MATRIX DIMENSION                       |
!     |                                                        |
!     |         W     --WORK ARRAY (LENGTH AT LEAST 4N)        |
!     |                                                        |
!     |    OUTPUT:                                             |
!     |                                                        |
!     |         E     --IMPROVED ESTIMATE FOR EIGENVALUE       |
!     |                                                        |
!     |         X     --EIGENVECTOR                            |
!     |                                                        |
!     |    BUILTIN FUNCTIONS: ABS,SQRT                         |
!     |________________________________________________________|
!
      USE stel_kinds
      IMPLICIT NONE
!-----------------------------------------------
!   D u m m y   A r g u m e n t s
!-----------------------------------------------
      INTEGER :: N
      REAL(rprec):: E
      REAL(rprec), DIMENSION(N) :: X, L, D, U
      REAL(rprec), DIMENSION(4*N) :: W
!-----------------------------------------------
!   L o c a l   P a r a m e t e r s
!-----------------------------------------------
      REAL(rprec), PARAMETER :: zero = 0, one = 1
!-----------------------------------------------
!   L o c a l   V a r i a b l e s
!-----------------------------------------------
      INTEGER :: G1, J1, J2, J4, J3, F, G, H, I, J, K, M
      REAL(rprec) :: O, P, Q, R, S, T, V, Y, Z
!-----------------------------------------------
      IF (N .LE. 1) THEN
         E = D(1)
         X(1) = ONE
         RETURN
      ENDIF
      M = N - 1
      J = 2
!     ---------------------------------------------------------
!     |*** STORE MATRIX IN W ARRAY AND SUBTRACT EIGENVALUE ***|
!     ---------------------------------------------------------
      X(1:M) = 0
      DO I = 1,M
         W(J) = U(I)
         W(J+1) = D(I) - E
         W(J+2) = L(I)
         J = J + 4
      END DO
      W(J) = 0
      W(J+1) = D(N) - E
      O = 65536
      O = O**(-4)
      T = O/2
      S = T
      T = T/2
      P = S
      S = S + T
      IF (S .GE. O) GO TO 40
      DO WHILE(S + T > S)
         T = T/2
         P = S
         S = S + T
         IF (S .GE. O) GO TO 40
      END DO
   40 CONTINUE
      R = W(3)
      V = ABS(R) + ABS(W(4))
      F = 4
      M = 4*N - 3
      G = -2
!     ---------------------------
!     |*** FACTOR THE MATRIX ***|
!     ---------------------------
      G = G + 4
      G1 = G
      DO G = G1, M, 4
         H = G - 1
         I = G + 2
         J = G + 5
!     --------------------
!     |*** FIND PIVOT ***|
!     --------------------
         Q = W(I)
         Y = ABS(Q)
         Z = ABS(R)
         IF (Z < Y) THEN
!     -------------------
!     |*** SWAP ROWS ***|
!     -------------------
            IF (V < Y) THEN
               V = Y
               F = I
            ENDIF
            T = W(G)
            W(G) = W(J)
            W(J) = T
            T = R/Q
            K = G + 1
            W(K) = Q
            K = J - 1
            S = W(K)
            IF (S .NE. ZERO) THEN
               IF (S .EQ. O) S = P
               W(K) = -S*T
               W(H) = S
               GO TO 100
            ENDIF
            W(K) = S
            W(H) = O
            GO TO 100
         ENDIF
         W(H) = 0
         IF (V < Z) THEN
            V = Z
            F = I
         ENDIF
         IF (R .EQ. ZERO) GO TO 120
         T = Q/R
!     -------------------
!     |*** ELIMINATE ***|
!     -------------------
  100    CONTINUE
         R = W(J) - T*W(G)
         W(J) = R
         W(I) = T
      END DO
      IF (ABS(R) < V) THEN
         V = R
         F = J + 1
!     ---------------------------------------------------
!     |*** COMPUTE INITIAL EIGENVECTOR APPROXIMATION ***|
!     ---------------------------------------------------
      ENDIF
  120 CONTINUE
      J = F/4
      X(J) = ONE
      IF (J .NE. 1) THEN
         K = F - 5
         J = J - 1
         X(J) = (X(J)-W(K-1)*X(J+1))/W(K)
         J1 = J
         IF (J1 - 1 > 0) THEN
            DO J = J1, 2, -1
               K = K - 4
               T = W(K-2)
               IF (T .EQ. O) T = 0
               X(J-1) = (X(J-1)-W(K-1)*X(J)-T*X(J+1))/W(K)
            END DO
         ENDIF
         GO TO 140
      ENDIF
  140 CONTINUE
      IF (V .NE. ZERO) THEN
         S = MAXVAL(ABS(X(:N)))
         S = ONE/S
         X(:N) = S*X(:N)
         R = SUM(X(:N)*X(:N))
         K = 0
         J = 1
         Y = X(1)
!     -----------------------------------------------------
!     |*** APPLY ONE ITERATION OF INVERSE POWER METHOD ***|
!     -----------------------------------------------------
         J2 = J
         J4 = MAX(N - 1,J2)
         DO J = J2, J4
            K = K + 4
            I = J
            S = W(K)
            W(K) = Y
            Y = X(J+1)
            IF (W(K-3) .NE. ZERO) THEN
               T = X(J+1)
               X(J+1) = X(I)
               X(I) = T
            ENDIF
            X(J+1) = X(J+1) - S*X(I)
         END DO
!     ---------------------------
!     |*** BACK SUBSTITUTION ***|
!     ---------------------------
         S = X(J)/W(K+3)
         X(J) = S
         T = ABS(S)
         V = S*Y
         J = J - 1
         K = K - 1
         S = (X(J)-W(K-1)*S)/W(K)
         X(J) = S
         T = MAX(ABS(S),T)
         V = V + S*W(K+1)
         J3 = J
         IF (J3 - 1 > 0) THEN
            DO J = J3, 2, -1
               K = K - 4
               Z = W(K-2)
               IF (Z .EQ. O) Z = 0
               S = (X(J-1)-W(K-1)*S-Z*X(J+1))/W(K)
               X(J-1) = S
               T = MAX(ABS(S),T)
               V = V + S*W(K+1)
            END DO
         ENDIF
         IF (V .NE. ZERO) V = R/V
         T = ONE/T
         S = SUM((X(:N)*V)**2)
         Z = SUM((T*X(:N))**2)
         T = T/SQRT(Z)
         X(:N) = T*X(:N)
!     --------------------------------------------------------------
!     |*** USE RAYLEIGH QUOTIENT TO IMPROVE EIGENVALUE ESTIMATE ***|
!     --------------------------------------------------------------
         IF (R + R .GE. S) E = E + V
         RETURN
      ENDIF
      T = MAXVAL(ABS(X(:N)))
      T = ONE/T
      Z = SUM((T*X(:N))**2)
      T = T/SQRT(Z)
      X(:N) = T*X(:N)

      END SUBROUTINE TVECT


module subspace
use, intrinsic:: iso_fortran_env, only: stderr=>error_unit
use, intrinsic:: iso_c_binding, only: c_int
use comm, only: sp, dp,pi, debug
use covariance,only: autocov_r, autocov_c
!use perf, only : sysclock2ms

implicit none (type, external)

! interface esprit
! module procedure esprit_c, esprit_r
! end interface esprit
!! not using due to f2py incompatibility

private
public :: esprit_c, esprit_r

contains

subroutine esprit_c(x,N,L,M,fs,tones,sigma) bind(c)

integer(c_int), intent(in) :: L,M,N
complex(dp),intent(in) :: x(N)
real(dp),intent(in) :: fs
real(dp),intent(out) :: tones(L),sigma(L)

integer :: LWORK
complex(dp) :: R(M,M), U(M,M), VT(M,M), S1(M-1,L), S2(M-1,L)
real(dp) :: S(M,M), RWORK(8*M), ang(L)
integer :: getrfinfo,getriinfo, evinfo, svdinfo
complex(dp) :: W1(L,L), IPIV(M-1), SWORK(8*M) !yes, this swork is complex
complex(dp) :: Phi(L,L), CWORK(8*M), junk(L,L), eig(L)

external :: zgesvd, zgetrf, zgetri, zgeev

! integer(i64) :: tic,toc

Lwork = 8*M !at least 5M for sgesvd
!------ estimate autocovariance from single time sample vector (1-D)
!call system_clock(tic)
call autocov_c(x, size(x,kind=c_int), M, R)
!call system_clock(toc)
!if (sysclock2ms(toc-tic).gt.1) write(stdout,*) 'ms to compute autocovariance estimate:',sysclock2ms(toc-tic)

!-------- SVD -------------------
!call system_clock(tic)
call zgesvd('A','N',M,M,R,M,S,U,M,VT,M,SWORK,LWORK,RWORK,svdinfo)
if (svdinfo /= 0) then
  write(stderr,*) 'ZGESVD return code',svdinfo
  error stop 'GESVD'
endif
!call system_clock(toc)
!if (sysclock2ms(toc-tic).gt.1.) write(stdout,*) 'ms to compute SVD:',sysclock2ms(toc-tic)

S1 = U(1:M-1, :L)
S2 = U(2:M, :L)

!call system_clock(tic)
W1=matmul(conjg(transpose(S1)),S1)
call zgetrf(L,L,W1,L,ipiv,getrfinfo) !LU decomp
if (getrfinfo /= 0) then
  write(stderr,*) 'ZGETRF inverse output code',getrfinfo
  error stop 'GETRF'
endif

call zgetri(L,W1,L,ipiv,Swork,Lwork,getriinfo) !LU inversion
if (getriinfo /= 0) then
  write(stderr,*) 'ZGETRI output code',getriinfo
  error stop 'GETRI'
endif

Phi = matmul(matmul(W1, conjg(transpose(S1))), S2)
!call system_clock(toc)
!if (sysclock2ms(toc-tic).gt.1.) write(stdout,*) 'ms to compute Phi via LU inv():',sysclock2ms(toc-tic)

!call system_clock(tic)
call zgeev('N','N',L,Phi,L,eig,junk,L,junk,L,cwork,lwork,rwork,evinfo)
if (evinfo /= 0) then
  write(stderr,*) 'ZGEEVS output code',evinfo
  error stop 'GEEV'
endif
!call system_clock(toc)
!if (sysclock2ms(toc-tic).gt.1.) write(stdout,*) 'ms to compute eigenvalues:',sysclock2ms(toc-tic)

ang = atan2(aimag(eig),real(eig))

tones = abs(fs*ang/(2*pi))

!eigenvalues  - NOT diag!!
sigma(:) = S(:L/2,1)

end subroutine esprit_c

subroutine esprit_r(x,N,L,M,fs,tout,sigma) bind(c)

integer(c_int), intent(in) :: L,M,N
real(sp),intent(in) :: x(N)
real(sp),intent(in) :: fs
real(sp),intent(out) :: tout(L/2),sigma(L)

real(sp) :: tones(L)
integer :: lwork

real, parameter :: pi = 4._sp * atan(1._sp)

!    integer, parameter :: Lwork=4096
integer,parameter :: LRATIO=8

real(sp) :: R(M,M), U(M,M),VT(M,M), S1(M-1,L), S2(M-1,L)
real(sp) :: S(M,M),RWORK(LRATIO*M),ang(L),SWORK(LRATIO*M) !this Swork is real
integer(c_int) :: getrfinfo,getriinfo, evinfo, svdinfo
real(sp) :: W1(L,L), IPIV(M-1)
complex(sp) :: Phi(L,L), CWORK(LRATIO*M), junk(L,L), eig(L)

external :: sgesvd, sgetrf, sgetri, cgeev

LWORK = LRATIO*M  !at least 5M for sgesvd
   ! integer(i64) :: tic,toc

!------ estimate autocovariance from single time sample vector (1-D)
!call system_clock(tic)
call autocov_r(x,size(x,kind=c_int),M,R)
!call system_clock(toc)
!if (sysclock2ms(toc-tic).gt.1) write(stdout,*) 'ms to compute autocovariance estimate:',sysclock2ms(toc-tic)

!-------- SVD -------------------
!call system_clock(tic)
!http://www.netlib.org/lapack/explore-html/d4/dca/group__real_g_esing.html
if (debug) print *,'LWORK: ',LWORK
call sgesvd('A','N',M,M,R,M,S,U,M,VT,M, SWORK, LWORK,svdinfo)
!if (debug) print *,'work(1):',swork(1)
if (svdinfo /= 0) then
  write(stderr,*) 'SGESVD return code',svdinfo,'  LWORK:',LWORK,'  M:',M
  if (M /= LWORK/LRATIO) error stop 'possible LWORK overflow'
endif
!call system_clock(toc)
!if (sysclock2ms(toc-tic).gt.1.) write(stdout,*) 'ms to compute SVD:',sysclock2ms(toc-tic)

S1 = U(1:M-1, :L)
S2 = U(2:M, :L)

!call system_clock(tic)
W1=matmul(transpose(S1),S1)

call sgetrf(L,L,W1,L,ipiv,getrfinfo) !LU decomp
if (getrfinfo /= 0) then
  write(stderr,*) 'SGETRF inverse output code',getrfinfo
  error stop 'GETRF'
endif

call sgetri(L,W1,L,ipiv,Rwork,Lwork,getriinfo) !LU inversion
if (getriinfo /= 0) then
  write(stderr,*) 'SGETRI output code',getriinfo
  error stop 'GETRI'
endif

!call sgemm('N','T',L,L,max(L,N-1),1.0,W1,L,S1,L,1.0,Phi,L)
!call sgemm('N','N',L,L,L,1.0,Phi,L,S2,L,1.,Phi,L)
Phi = matmul(matmul(W1, transpose(S1)), S2)

!call system_clock(toc)
!if (sysclock2ms(toc-tic).gt.1.) write(stdout,*) 'ms to compute Phi via LU inv():',sysclock2ms(toc-tic)

!call system_clock(tic)
call cgeev('N','N',L,Phi,L,eig,junk,L,junk,L,cwork,lwork,rwork,evinfo)
if (evinfo /= 0) then
  write(stderr,*) 'CGEEV output code',evinfo
  error stop 'GEEV'
endif
!call system_clock(toc)
!if (sysclock2ms(toc-tic).gt.1.) write(stdout,*) 'ms to compute eigenvalues:',sysclock2ms(toc-tic)

ang = atan2(aimag(eig),real(eig))

tones = abs(fs*ang/(2*pi))

tout = tones(1:L:2)

!eigenvalues  - NOT diag!!
sigma(:) = S(:L/2,1)

end subroutine esprit_r

end module subspace


module eqdsk
    use init_mpi, only : mpi_pId

    implicit none
    save
    character (len=10) :: case_ ( 6 )
    integer :: idum, nw, nh, nbbbs, limitr
    real :: rdim, zdim, rcentr, rleft, zmid, &
        rmaxis, zmaxis, simag, sibry, bcentr, &
        current, xdum, rStep, zStep, fStep

    real, allocatable :: fpol (:), psizr (:,:), &
        pres (:), ffprim (:), pprime (:), qpsi (:), &
        rbbbs (:), zbbbs (:), rlim (:), zlim (:), &
        r (:), z (:), bR(:,:), bPhi(:,:), bz(:,:), &
        fluxGrid (:), fpolRZ(:,:), bMag(:,:), &
        fluxGrid_(:), fpol_(:)

    logical :: ascending_flux

contains
    subroutine read_geqdsk ( eqdsk_fileName, plot )
        !use dislin
        use dlg

        implicit none

        integer :: i, j, iErr
        character(len=*), intent(IN) :: eqdsk_fileName
        logical, intent(IN), optional :: plot
        real, allocatable :: yp(:), xp(:), temp(:), ss(:), yp_c(:)
        real :: spl1 = 0.0, spln = 0.0, t, curv2, sigma = 0.0

        !!   Plotting variables

        !integer :: nLevs
        !real :: levStep, lev

        !   Read in variables from geqdsk file

        open ( unit = 8, file = eqdsk_fileName, status = 'OLD' )

        read ( 8, 2000 ) ( case_ (i), i=1, 6 ), idum, nw, nh
        if(mpi_pId==1) write(*,*) 'EQDSK header data: nw, nh', nw, nh
        read ( 8, 2020 ) rdim,zdim,rcentr,rleft,zmid
        read ( 8, 2020 ) rmaxis,zmaxis,simag,sibry,bcentr
        read ( 8, 2020 ) current,simag,xdum,rmaxis,xdum
        read ( 8, 2020 ) zmaxis,xdum,sibry,xdum,xdum

        allocate ( fpol ( nw ), pres ( nw ), ffprim ( nw ), &
            pprime ( nw ), psizr ( nw, nh ), qpsi ( nw ), &
            r ( nw ), z ( nh ), fluxGrid ( nw ), &
            fluxGrid_(nw), fpol_(nw) )

        read ( 8, 2020 ) ( fpol (i), i=1, nw )
        read ( 8, 2020 ) ( pres (i), i=1, nw )
        read ( 8, 2020 ) ( ffprim (i), i=1, nw )
        read ( 8, 2020 ) ( pprime (i), i=1, nw )
        read ( 8, 2020 ) ( ( psizr (i,j), i=1, nw ), j=1, nh )
        read ( 8, 2020 ) ( qpsi (i), i=1, nw )

        read ( 8, 2022 ) nbbbs,limitr

        allocate ( rbbbs ( nbbbs ), zbbbs ( nbbbs ), &
            rlim ( limitr ), zlim ( limitr ) )

        read ( 8, 2020 ) ( rbbbs (i), zbbbs (i), i=1,nbbbs )
        read ( 8, 2020 ) ( rlim (i), zlim (i), i=1,limitr )

        2000 format (6a8,3i4)
        2020 format (5e16.9)
        2022 format (2i5)

        close ( unit = 8 )

        !   Calculate other required variables

        rStep   = rdim / ( nw - 1 )
        zStep   = zdim / ( nh - 1 )
        fStep   = ( sibry - simag ) / ( nw - 1 )

        r   = (/ (i,i=0,nw-1) /) * rStep + rleft
        z   = (/ (i,i=0,nh-1) /) * zStep + zmid - zdim / 2.0

        fluxGrid    = (/ (i,i=0,nW-1) /) * fStep + simag

        allocate ( bR ( nw, nh ), bz ( nw, nh ), &
            bPhi ( nw, nh ), bMag(nw,nh) )

        bR  = dlg_pDeriv ( psizr, 2, zStep ) * 1.0
        bz  = -1.0 * dlg_pDeriv ( psizr, 1, rStep ) * 1.0

        !   Remember psi = - R * A

        do i=1,nw
            do j=1,nh
                bR(i,j)  = -bR(i,j) / r(i)
                bz(i,j)  = -bz(i,j) / r(i)
            enddo
        enddo

        allocate ( xp(nw), yp(nw), temp(nw), ss(nw), fpolRZ(nw,nh), yp_c(nw) )
        !   curv1 initialises the spline (fitpack.f)

!        call kurv1 ( nw, fluxGrid, fpol, spl1, spln, 3, xp, yp, temp, ss, 0.0, iErr )

        !   force the fluxGrid to have an ascending order
        ascending_flux = .true.
        if ( siMag > siBry ) ascending_flux = .false.

        if ( .not. ascending_flux ) then
            write(*,*) 'NOTE:  Reversing the flux grid for curv1 (PERHAPS AN ITER EQDSK?)'
            do i=1,nw
                fluxGrid_(i)    = fluxGrid(nw-i+1)
                fpol_(i)    = fpol(nw-i+1)
            enddo
            fluxGrid    = fluxGrid_
            fpol    = fpol_
        endif

        call curv1 ( nw, fluxGrid, fpol, spl1, spln, 3, yp_c, temp, sigma, iErr )

        do i=1,nw
            do j=1,nh

                !   curv2 evaluates the spline (fitpack.f)
                t   =  ( psizr(i,j) - simag ) / ( sibry - simag )
                !call kurv2 ( t, xs, ys, nw, fluxGrid, fpol, xp, yp, ss, 0.0 )
                !fPolRZ(i,j) = ys

                fPolRZ(i,j) = curv2 ( psizr(i,j), nw, fluxGrid, fpol, &
                        yp_c, sigma )

                bPhi(i,j)   = fpolRZ(i,j) / r(i)
            end do
        end do

!        !   Test the fitpack interpolation
!
!        do i=1,nw
!
!            t   =  ( fluxGrid(i) - simag ) / ( sibry - simag )
!!            call kurv2 ( t, xs, ys, nw, fluxGrid, fpol, xp, yp, ss, 0.0 )
!            write (*,*) i, t, fluxGrid(i), fpol(i), xs, ys, &
!                curv2 ( fluxGrid(i), nw, fluxGrid, fpol, yp_c, 0.0 )
!
!        end do

        bMag    = sqrt ( bR**2 + bPhi**2 + bz**2 )

        !!   Plotting

        !if ( present ( plot ) ) then
        !    if ( plot ) then
        !
        !        call setFil ( 'eqdsk.eps' )
        !        call setPag ( 'DA4P' )
        !        call metaFl ( 'EPS' )
        !        call disIni ()
        !        call graf ( 0.0, 3.0, 0.0, 0.5, -2.0, 2.0, -2.0, 0.5 )
        !        nLevs   = 101
        !        levStep    = ( maxVal ( psizr ) - minVal ( psizr ) ) / nLevs
        !        do i=0,nLevs-1
        !            lev    = i * levStep - nLevs/2*levStep
        !            call contur ( r, nw, z, nh, psizr, lev )
        !        end do
        !        call endGrf ()
        !        call disFin ()

        !    end if
        !end if

    end subroutine read_geqdsk

end module eqdsk


MODULE rsdft_fft_module

  implicit none

  PRIVATE
  PUBLIC :: rsdft_fft3d

  integer :: mm
  real(8),parameter :: pi =3.14159265358979323846264338327950d0
 !real(8),parameter :: pi2=6.28318530717958647692528676655901d0
  real(8) :: pi2
  complex(8),parameter :: z0=(0.0d0,0.0d0)
  complex(8),parameter :: zi=(0.0d0,1.0d0)

CONTAINS

  SUBROUTINE rsdft_fft3d( zf, forward_or_backward )
    implicit none
    complex(8),intent(INOUT) :: zf(0:,0:,0:)
    integer,intent(IN) :: forward_or_backward
    complex(8),allocatable :: a(:),b(:),c(:)
    integer :: ml(3),n,i,j,k
    real(8) :: d

    ml(1) = size( zf, 1 )
    ml(2) = size( zf, 2 )
    ml(3) = size( zf, 3 )

    pi2 = 2.0d0*pi
    if ( forward_or_backward < 0 ) pi2=-pi2

    n = maxval( ml )
    allocate( a(0:n-1) ) ; a=z0
    allocate( b(0:n-1) ) ; b=z0
    allocate( c(0:n-1) ) ; c=z0

    do k=0,ml(3)-1
    do j=0,ml(2)-1
       a(:)=z0
       b(:)=z0
       c(:)=z0
       do i=0,ml(1)-1
          a(i) = zf(i,j,k)
       end do
       call rsdft_fft( ml(1), a, b, c )
       do i=0,ml(1)-1
          zf(i,j,k) = b(i)
       end do
    end do ! j
    end do ! k

    do k=0,ml(3)-1
    do i=0,ml(1)-1
       a(:)=z0
       b(:)=z0
       c(:)=z0
       do j=0,ml(2)-1
          a(j) = zf(i,j,k)
       end do
       call rsdft_fft( ml(2), a, b, c )
       do j=0,ml(2)-1
          zf(i,j,k) = b(j)
       end do
    end do ! i
    end do ! k

    do j=0,ml(2)-1
    do i=0,ml(1)-1
       a(:)=z0
       b(:)=z0
       c(:)=z0
       do k=0,ml(3)-1
          a(k) = zf(i,j,k)
       end do
       call rsdft_fft( ml(3), a, b, c )
       do k=0,ml(3)-1
          zf(i,j,k) = b(k)
       end do
    end do ! i
    end do ! j

    if ( forward_or_backward >= 0 ) then
       d = 1.0d0/dble( ml(1)*ml(2)*ml(3) )
       zf(:,:,:) = d*zf(:,:,:)
    end if

    deallocate( c )
    deallocate( b )
    deallocate( a )

  END SUBROUTINE rsdft_fft3d

  SUBROUTINE rsdft_fft( n, a, b, c )
    implicit none
    integer,intent(IN) :: n
    complex(8),intent(INOUT) :: a(0:n-1)
    complex(8),intent(INOUT) :: b(0:)
    complex(8),intent(INOUT) :: c(0:n-1)
    integer :: i,j,k
    integer,allocatable :: info_fact(:),lst_fact(:)
    integer,allocatable :: indx(:)

    allocate( info_fact(2:n) ) ; info_fact=0
    call prime_factor_decomposition( n, info_fact )

    k=sum( info_fact )
    allocate( lst_fact(k) ) ; lst_fact=0

    k=0
    do i=n,2,-1
       do j=1,info_fact(i)
          k=k+1
          lst_fact(k) = i
       end do
    end do

    allocate( indx(0:n-1) ) ; indx=0

    mm=0
    call fft_1_indx( n, lst_fact, indx )
    mm=0
    call fft_1_sub( n, lst_fact, a, b, c )

    c(:)=b(:)
    do i=0,n-1
       j=indx(i)
       b(j)=c(i)
    end do

    deallocate( indx )
    deallocate( lst_fact  )
    deallocate( info_fact )

  END SUBROUTINE rsdft_fft

  SUBROUTINE prime_factor_decomposition( n0, ifact )
    implicit none
    integer,intent(IN)  :: n0
    integer,intent(OUT) :: ifact(2:n0)
    integer :: n,i,j,m,t
    ifact(:)=0
    n=n0
    m=n
    loop_j : do j=1,n0
       loop_i : do i=2,n
          t = mod( m, i )
          if ( t == 0 ) then
             m = m/i
             ifact(i) = ifact(i) + 1
             exit loop_i
          else if ( i == n ) then
             exit loop_j
          end if
       end do loop_i
       n=m
    end do loop_j
  END SUBROUTINE prime_factor_decomposition

  RECURSIVE SUBROUTINE fft_1_sub( n, lst_fact, a, b, c )
    implicit none
    integer,intent(IN) :: n, lst_fact(:)
    complex(8),intent(INOUT) :: a(0:n-1)
    complex(8),intent(INOUT) :: b(0:)
    complex(8),intent(INOUT) :: c(0:)
    integer :: i,j,k,n1,n2

    if ( power_2_check(n) ) then
       call rsdft_fft_2( n, a, b, c )
       return
    end if

    n1 = lst_fact(1)
    n2 = n/n1

    do k=0,n1-1

       c(:)=z0
       do i=0,n2-1
          do j=0,n1-1
             c(i) = c(i) + a(i+j*n2)*exp( -zi*pi2*k/n*(i+j*n2) )
          end do
       end do

       if ( size(lst_fact) == 1 ) then
          do j=0,n2-1
             do i=0,n2-1
                b(mm) = b(mm) + c(i)*exp( -zi*pi2*j/n2*i )
             end do
             mm=mm+1
          end do
       else
          call fft_1_sub( n2, lst_fact(2:), c, b, c(n2:) )
       end if

    end do ! k

  END SUBROUTINE fft_1_sub

  RECURSIVE SUBROUTINE fft_1_indx( n, lst_fact, indx )
    implicit none
    integer,intent(IN) :: n, lst_fact(:)
    integer,intent(INOUT) :: indx(0:)
    integer :: j,k,n1,n2,mm0

    if ( power_2_check(n) ) then
       do j=0,n-1
          indx(j+mm) = bit_reverse( j, n )
       end do
       mm=mm+n
       return
    end if

    n1 = lst_fact(1)
    n2 = n/n1

    do k=0,n1-1
       if ( size(lst_fact) == 1 ) then
          do j=0,n2-1
             indx(mm)=j*n1+k
             mm=mm+1
          end do
       else
          mm0=mm
          call fft_1_indx( n2, lst_fact(2:), indx )
          do j=mm0,mm-1
             indx(j)=indx(j)*n1+k
          end do
       end if
    end do ! k

  END SUBROUTINE fft_1_indx

  RECURSIVE SUBROUTINE rsdft_fft_2( n, a, b, c )
    implicit none
    integer,intent(IN) :: n
    complex(8),intent(INOUT) :: a(0:n-1)
    complex(8),intent(INOUT) :: b(0:)
    complex(8),intent(INOUT) :: c(0:n-1)
    integer :: i

    do i=0,n/2-1
       c(i) = a(i) + a(i+n/2)
    end do

    if ( n == 2 ) then
       b(mm)=c(0)
       mm=mm+1
    else
       call rsdft_fft_2( n/2, c, b, c(n/2) )
    end if

    do i=0,n/2-1
       c(i) = ( a(i) - a(i+n/2) )*exp( -pi2*zi/n*i )
    end do

    if ( n == 2 ) then
       b(mm)=c(0)
       mm=mm+1
    else
       call rsdft_fft_2( n/2, c, b, c(n/2) )
    end if

  END SUBROUTINE rsdft_fft_2

  FUNCTION power_2_check( n )
    implicit none
    logical :: power_2_check
    integer,intent(IN)  :: n
    real(8) :: x
    integer :: i
    x = log(dble(n))/log(2.0d0)
    i = nint(x)
    if ( abs(x-i) < 1.d-10 ) then
       power_2_check = .true.
    else
       power_2_check = .false.
    end if
  END FUNCTION power_2_check

  FUNCTION bit_reverse( i, n )
    implicit none
    integer :: bit_reverse
    integer,intent(IN) :: i, n
    integer :: len_bit,ipos,i_br
    len_bit = nint( log(dble(n))/log(2.0d0) )
    i_br = 0
    do ipos=0,len_bit-1
       call mvbits( i, ipos, 1, i_br, len_bit-1-ipos )
    end do
    bit_reverse = i_br
  END FUNCTION bit_reverse

END MODULE rsdft_fft_module


!	Find estimated reliabilities of eaps of latent vectors.
!	Input:

!	covtheta is the array of conditional covariance matrices.
!	meantheta is the array of conditional means.

!	Output:

!	obsweight provides observation weights.
!	coveap is the covariance matrix of the eaps.
!	covartheta is the covariance matrix of the latent vector.
!	meancovtheta is the average conditional covariance of theta given the observations.
!	meaneap is the average eap.
!	releap is the reliability for each element of the latent vector.

subroutine reltheta(covtheta,meantheta,obsweight,coveap,covartheta,meancovtheta,meaneap,releap)
implicit none
interface
!	Multinomial mean.
	function mmeanth(density,quadpoint)
		implicit none
		real(kind=8),intent(in)::density(:),quadpoint(:,:)
		real(kind=8)::mmeanth(size(quadpoint,1))
	end function mmeanth
!	Multinomial covariance matrix.
	function mcovth(density,meanth,quadpoint)
		implicit none
		real(kind=8),intent(in)::density(:),meanth(:),quadpoint(:,:)
		real(kind=8)::mcovth(size(meanth),size(meanth))
	end function mcovth

end interface
real(kind=8),intent(in)::covtheta(:,:,:),meantheta(:,:),obsweight(:)
real(kind=8),intent(out)::coveap(:,:),covartheta(:,:),meancovtheta(:,:),meaneap(:),releap(:)
!	row is row number.
integer::row
!	Average conditional variance, mean eap, variance of eaps, total weight
real(kind=8)::condvar,mean,var,total
total=sum(obsweight)
releap=0.0_8
meaneap=mmeanth(obsweight,meantheta)/total
coveap=mcovth(obsweight,meaneap,meantheta)/total
do row=1,size(meaneap)
	meancovtheta(:,row)=mmeanth(obsweight,covtheta(:,row,:))/total
	covartheta(:,row)=meancovtheta(:,row)+coveap(:,row)
	if(covartheta(row,row)>0.0_8)releap(row)=coveap(row,row)/covartheta(row,row)
end do

return
end subroutine reltheta


      subroutine bsplpp ( t, bcoef, n, k, scrtch, break, coef, l )
!  from  * a practical guide to splines *  by c. de Boor (7 may 92)
!alls  bsplvb
!
!onverts the b-representation  t, bcoef, n, k  of some spline into its
!  pp-representation  break, coef, l, k .
!
!******  i n p u t  ******
!  t.....knot sequence, of length  n+k
!  bcoef.....b-spline coefficient sequence, of length  n
!  n.....length of  bcoef  and  dimension of spline space  spline(k,t)
!  k.....order of the spline
!
!  w a r n i n g  . . .  the restriction   k .le. kmax (= 20)   is impo-
!        sed by the arbitrary dimension statement for  biatx  below, but
!        is  n o w h e r e   c h e c k e d   for.
!
!******  w o r k   a r e a  ******
!  scrtch......of size  (k,k) , needed to contain bcoeffs of a piece of
!        the spline and its  k-1  derivatives
!
!******  o u t p u t  ******
!  break.....breakpoint sequence, of length  l+1, contains (in increas-
!        ing order) the distinct points in the sequence  t(k),...,t(n+1)
!  coef.....array of size (k,l), with  coef(i,j) = (i-1)st derivative of
!        spline at break(j) from the right
!  l.....number of polynomial pieces which make up the spline in the in-
!        terval  (t(k), t(n+1))
!
!******  m e t h o d  ******
!     for each breakpoint interval, the  k  relevant b-coeffs of the
!  spline are found and then differenced repeatedly to get the b-coeffs
!  of all the derivatives of the spline on that interval. the spline and
!  its first  k-1  derivatives are then evaluated at the left end point
!  of that interval, using  bsplvb  repeatedly to obtain the values of
!  all b-splines of the appropriate order at that point.
!
      integer k,l,n,   i,j,jp1,kmax,kmj,left,lsofar
      parameter (kmax = 20)
      real(8) bcoef(n),break(l+1),coef(k,l),t(n+k),   scrtch(k,k) &
                                            ,biatx(kmax),diff,factor,sum
!
      lsofar = 0
      break(1) = t(k)
      do 50 left=k,n
!                                find the next nontrivial knot interval.
         if (t(left+1) .eq. t(left))    go to 50
         lsofar = lsofar + 1
         break(lsofar+1) = t(left+1)
         if (k .gt. 1)                  go to 9
         coef(1,lsofar) = bcoef(left)
                                        go to 50
!        store the k b-spline coeff.s relevant to current knot interval
!                             in  scrtch(.,1) .
    9    do 10 i=1,k
   10       scrtch(i,1) = bcoef(left-k+i)
!
!        for j=1,...,k-1, compute the  k-j  b-spline coeff.s relevant to
!        current knot interval for the j-th derivative by differencing
!        those for the (j-1)st derivative, and store in scrtch(.,j+1) .
         do 20 jp1=2,k
            j = jp1 - 1
            kmj = k - j
            do 20 i=1,kmj
               diff = t(left+i) - t(left+i - kmj)
               if (diff .gt. 0.)  scrtch(i,jp1) = &
                             (scrtch(i+1,j)-scrtch(i,j))/diff
   20          continue
!
!        for  j = 0, ..., k-1, find the values at  t(left)  of the  j+1
!        b-splines of order  j+1  whose support contains the current
!        knot interval from those of order  j  (in  biatx ), then comb-
!        ine with the b-spline coeff.s (in scrtch(.,k-j) ) found earlier
!        to compute the (k-j-1)st derivative at  t(left)  of the given
!        spline.
!           note. if the repeated calls to  bsplvb  are thought to gene-
!        rate too much overhead, then replace the first call by
!           biatx(1) = 1.
!        and the subsequent call by the statement
!           j = jp1 - 1
!        followed by a direct copy of the lines
!           deltar(j) = t(left+j) - x
!                  ......
!           biatx(j+1) = saved
!        from  bsplvb . deltal(kmax)  and  deltar(kmax)  would have to
!        appear in a dimension statement, of course.
!
         call bsplvb ( t, 1, 1, t(left), left, biatx )
         coef(k,lsofar) = scrtch(1,k)
         do 30 jp1=2,k
            call bsplvb ( t, jp1, 2, t(left), left, biatx )
            kmj = k+1 - jp1
            sum = 0.
            do 28 i=1,jp1
   28          sum = biatx(i)*scrtch(i,kmj) + sum
   30       coef(kmj,lsofar) = sum
   50    continue
      l = lsofar
        if (k .eq. 1)                     return
        factor = 1.
        do 60 i=2,k
             factor = factor*float(k+1-i)
             do 60 j=1,lsofar
   60       coef(i,j) = coef(i,j)*factor
                                        return
      end subroutine bsplpp