!***********************************************************************
! Compute potential solar radiation and sunlight hours for each HRU for
! each day of year; modification of soltab_prms
!
! References -- you *will* need these to figure out what is going on:
!   Swift, L.W., Jr., 1976, Algorithm for solar radiation on mountain
!   slopes: Water Resources Research, v. 12, no. 1, p. 108-112.
!
!   Lee, R., 1963, Evaluation of solar beam irradiation as a climatic parameter
!   of mountain watersheds, Colorado State University Hydrology Papers, 2,
!   50 pp.
!***********************************************************************
      MODULE PRMS_SOLTAB
      IMPLICIT NONE
!   Local Variables
      DOUBLE PRECISION, PARAMETER :: PI=3.1415926535898D0
      DOUBLE PRECISION, PARAMETER :: RADIANS=PI/180.0D0, TWOPI=2.0D0*PI
      DOUBLE PRECISION, PARAMETER :: PI_12=12.0D0/PI
! TWOPI ~ 6.2831853071786
! RADIANS ~ 0.017453292519943
! PI_12 ~ 3.8197186342055
      CHARACTER(LEN=6), SAVE :: MODNAME
      DOUBLE PRECISION, SAVE :: Solar_declination(366), Soltab_basinpotsw(366)
      DOUBLE PRECISION, SAVE, ALLOCATABLE :: Hru_cossl(:), Soltab_sunhrs(:, :)
!   Declared Variables
      DOUBLE PRECISION, SAVE, ALLOCATABLE :: Soltab_potsw(:, :), Soltab_horad_potsw(:, :)
!   Declared Parameters
      REAL, SAVE, ALLOCATABLE :: Hru_aspect(:), Hru_slope(:)
      END MODULE PRMS_SOLTAB

!***********************************************************************
!     Main soltab routine
!***********************************************************************
      INTEGER FUNCTION soltab()
      USE PRMS_MODULE, ONLY: Process
      IMPLICIT NONE
! Functions
      INTEGER, EXTERNAL :: sthdecl, sthinit
!***********************************************************************
      soltab = 0

      IF ( Process(:4)=='decl' ) THEN
        soltab = sthdecl()
      ELSEIF ( Process(:4)=='init' ) THEN
        soltab = sthinit()
      ENDIF

      END FUNCTION soltab

!***********************************************************************
!     sthdecl - set up parameters for solar radiation computations
!   Declared Parameters
!     hru_aspect, hru_lat, hru_slope
!***********************************************************************
      INTEGER FUNCTION sthdecl()
      USE PRMS_SOLTAB
      USE PRMS_MODULE, ONLY: Nhru
      IMPLICIT NONE
! Functions
      INTRINSIC INDEX
      INTEGER, EXTERNAL :: declparam, declvar
      EXTERNAL read_error, print_module
! Local Variables
      CHARACTER(LEN=80), SAVE :: Version_soltab
!***********************************************************************
      sthdecl = 0

      Version_soltab = 'soltab.f90 2016-09-09 12:53:00Z'
      CALL print_module(Version_soltab, 'Potential Solar Radiation   ', 90)
      MODNAME = 'soltab'

      ALLOCATE ( Soltab_potsw(366, Nhru) )
      IF ( declvar(MODNAME, 'soltab_potsw', 'ndays,nhru', 366*Nhru, 'double', &
     &     'Potential solar radiation for each Julian Day, for each HRU', &
     &     'Langleys', Soltab_potsw)/=0 ) CALL read_error(3, 'soltab_potsw')

      ALLOCATE ( Soltab_horad_potsw(366, Nhru) )
      IF ( declvar(MODNAME, 'soltab_horad_potsw', 'ndays,nhru', 366*Nhru, 'double', &
     &     'Potential solar radiation on a horizontal plane for each Julian Day, for each HRU', &
     &     'Langleys', Soltab_horad_potsw)/=0 ) CALL read_error(3, 'soltab_horad_potsw')

      ALLOCATE ( Hru_cossl(Nhru), Soltab_sunhrs(366, Nhru) )

!   Declared Parameters
      ALLOCATE ( Hru_slope(Nhru) )
      IF ( declparam(MODNAME, 'hru_slope', 'nhru', 'real', &
     &     '0.0', '0.0', '10.0', &
     &     'HRU slope', &
     &     'Slope of each HRU, specified as change in vertical length divided by change in horizontal length', &
     &     'decimal fraction')/=0 ) CALL read_error(1, 'hru_slope')

      ALLOCATE ( Hru_aspect(Nhru) )
      IF ( declparam(MODNAME, 'hru_aspect', 'nhru', 'real', &
     &     '0.0', '0.0', '360.0', &
     &     'HRU aspect', 'Aspect of each HRU', &
     &     'angular degrees')/=0 ) CALL read_error(1, 'hru_aspect')

      END FUNCTION sthdecl

!***********************************************************************
!     sthinit - Initialize soltab module - get parameter values,
!               compute soltab_potsw (potential shortwave radiation)
!               and soltab_sunhrs (hours between sunrise and sunset)
!               for each HRU for each day of the year.
!***********************************************************************
      INTEGER FUNCTION sthinit()
      USE PRMS_SOLTAB
      USE PRMS_MODULE, ONLY: Nhru, Print_debug
      USE PRMS_BASIN, ONLY: Hru_type, Active_hrus, Hru_route_order, Basin_lat, Hru_lat
      IMPLICIT NONE
! Functions
      INTRINSIC SIN, COS, DBLE
!     INTRINSIC ASIN
      INTEGER, EXTERNAL :: getparam
      EXTERNAL compute_soltab, read_error, PRMS_open_module_file
! Local Variables
      CHARACTER(LEN=12) :: output_path
      INTEGER :: jd, j, n, file_unit, nn
      REAL :: lat
      DOUBLE PRECISION :: basin_cossl
      DOUBLE PRECISION :: basin_sunhrs(366), obliquity(366)
      DOUBLE PRECISION :: y, y2, y3, jddbl
      DOUBLE PRECISION, PARAMETER :: ECCENTRICY = 0.01671D0
      DOUBLE PRECISION, PARAMETER :: DAYSYR = 365.242D0
      ! 0.016723401  daily change -1.115E-09, eccen = 0.016723401 + (julhour-julhour(1966,1,0,18))+dmin/60)/24*-1.115E-09
      ! julday(1966,1,0.75 UT) = 2439126.25
      ! eccen = 0.01675104-0.00004180*T-0.000000126*T^2  T is julian centuries (days time from epoch, is GMT from Jan 0.0
      DOUBLE PRECISION, PARAMETER :: DEGDAY = 360.0D0/DAYSYR
      DOUBLE PRECISION, PARAMETER :: DEGDAYRAD = DEGDAY*RADIANS ! about 0.00143356672
! DEGDAY = 360 degrees/days in year
!***********************************************************************
      sthinit = 0

      IF ( getparam(MODNAME, 'hru_slope', Nhru, 'real', Hru_slope)/=0 ) CALL read_error(2, 'hru_slope')
      IF ( getparam(MODNAME, 'hru_aspect', Nhru, 'real', Hru_aspect)/=0 ) CALL read_error(2, 'hru_aspect')

      DO jd = 1, 366
        jddbl = DBLE(jd)

        ! cosine of the solar zenith angle: http://www.cesm.ucar.edu/models/cesm1.0/cesm/cesmBbrowser/html_code/csm_share/shr_orb_mod.F90.html
        !    jday   ! Julian cal day (1.xx to 365.xx)
        !    lat    ! Centered latitude (radians)
        !    lon    ! Centered longitude (radians)
        !    declin ! Solar declination (radians)
        ! shr_orb_cosz = sin(lat)*sin(declin) - &
   !&              cos(lat)*cos(declin)*cos(jday*2.0_SHR_KIND_R8*pi + lon)

        ! Eccentricity from equation E-2 (Dingman, S. L., 1994, Physical Hydrology. Englewood Cliffs, NJ: Prentice Hall, 575 p.)
        ! eccentricity = (r_0/r)^2 = 1.00011+0.034221 cosΓ+0.00128 sinΓ+0.000719 cos2Γ+0.000077 sin2Γ
        ! Γ = (2π(J-1))/365 = DEGDAYRAD*(jddbl-1.0D0) = day angle
        ! dayangle = DEGDAYRAD*(jddbl-1.0D0)
        ! eccentricity = 1.00011D0 + 0.034221D0*COS(dayangle) + 0.00128D0*SIN(dayangle) + 0.000719D0*COS(2.0D0*dayangle) + 0.000077D0*SIN(2.0D0*dayangle)
!rsr .0172 = 2PI/365 = RADIAN_YEAR = DEGDAYRAD
!rsr01/2006 commented out equations from Llowd W. Swift paper 2/1976
!       obliquity(jd) = 1.0D0 - (0.0167D0*COS((jd-3)*0.0172D0))
        obliquity(jd) = 1.0D0 - (ECCENTRICY*COS((jddbl-3.0D0)*DEGDAYRAD))
!       Solar_declination(jd) = 0.007D0 - (0.4067D0*COS((jd+10)*0.0172D0))
!       Solar_declination(jd) = ASIN(0.39785D0 * SIN( (278.9709D0+DEGDAY*jd)*RADIANS + 1.9163D0*RADIANS * SIN((356.6153D0+DEGDAY*jd)*RADIANS )) )
        ! hour = 12.0D0
!       y = DEGDAYRAD*(jddbl-1.0D0 +(hour-12.0D0)/24.0D0)
        y = DEGDAYRAD*(jddbl-1.0D0) ! assume noon
        y2 = 2.0D0*y
        y3 = 3.0D0*y
        Solar_declination(jd) = 0.006918D0 - 0.399912D0*COS(y) + 0.070257D0*SIN(y) &
     &                          - 0.006758D0*COS(y2) + 0.000907D0*SIN(y2) &
     &                          - 0.002697D0*COS(y3) + 0.00148D0*SIN(y3)
      ENDDO

!   Module Variables
      Soltab_sunhrs = 0.0D0
      Soltab_potsw = 0.0D0
      Soltab_horad_potsw = 0.0D0
      Hru_cossl = 0.0D0
      DO nn = 1, Active_hrus
        n = Hru_route_order(nn)
        CALL compute_soltab(obliquity, Solar_declination, 0.0, 0.0, Hru_lat(n), &
     &                      Hru_cossl(n), Soltab_horad_potsw(1, n), &
     &                      Soltab_sunhrs(1, n), Hru_type(n), n)
        CALL compute_soltab(obliquity, Solar_declination, Hru_slope(n), Hru_aspect(n), &
     &                      Hru_lat(n), Hru_cossl(n), Soltab_potsw(1, n), &
     &                      Soltab_sunhrs(1, n), Hru_type(n), n)
      ENDDO

      lat = SNGL( Basin_lat )
      CALL compute_soltab(obliquity, Solar_declination, 0.0, 0.0, lat, basin_cossl, &
     &                    Soltab_basinpotsw, basin_sunhrs, 0, 0)

      IF ( Print_debug==5 ) THEN
        output_path = 'soltab_debug'
        PRINT *, ''
        PRINT *, 'soltab debug data written to: ', output_path
        CALL PRMS_open_module_file(file_unit, output_path)
        DO n = 1, Nhru
          WRITE ( file_unit, * ) 'HRU:', n
          WRITE ( file_unit, * ) '***Soltab_sunhrs***'
          WRITE ( file_unit, '(13F8.3)' ) (Soltab_sunhrs(j,n), j=1,366)
          WRITE ( file_unit, * ) '***Soltab_potsw***'
          WRITE ( file_unit, '(13F8.3)' ) (Soltab_potsw(j,n), j=1,366)
        ENDDO
!       WRITE ( file_unit, * ) obliquity, Solar_declination
        WRITE ( file_unit, * ) 2.0D0/(obliquity(356)*obliquity(356)), 2.0D0/(obliquity(10)*obliquity(10)), &
     &                         2.0D0/(obliquity(23)*obliquity(23)), 2.0D0/(obliquity(38)*obliquity(38)), &
     &                         2.0D0/(obliquity(51)*obliquity(51)), 2.0D0/(obliquity(66)*obliquity(66)), &
     &                         2.0D0/(obliquity(80)*obliquity(80)), 2.0D0/(obliquity(94)*obliquity(94)), &
     &                         2.0D0/(obliquity(109)*obliquity(109)), 2.0D0/(obliquity(123)*obliquity(123)), &
     &                         2.0D0/(obliquity(138)*obliquity(138)), 2.0D0/(obliquity(152)*obliquity(152)), &
     &                         2.0D0/(obliquity(173)*obliquity(173))
        WRITE ( file_unit, * ) Solar_declination(356), Solar_declination(10), Solar_declination(23), &
     &                         Solar_declination(38), Solar_declination(51), Solar_declination(66), &
     &                         Solar_declination(80), Solar_declination(94), Solar_declination(109), &
     &                         Solar_declination(123), Solar_declination(138), Solar_declination(152), &
     &                         Solar_declination(173)
        CLOSE ( file_unit)
! from original soltab
!     data obliquity/2.06699,2.06317,2.05582,2.04520,2.03243,2.01706,2.00080,
!    +1.98553,1.96990,1.95714,1.94689,1.94005,1.93616/

!     data Solar_declination/-.410152,-.383391,-.337430,-.27198,-.190532,-.09832,0.,
!    +.09832,.190532,.27198,.33743,.383391,.410152/

!     data jday/356,10,23,38,51,66,80,94,109,123,138,152,173/
      ENDIF

      DEALLOCATE ( Hru_slope, Hru_aspect )

      END FUNCTION sthinit

!***********************************************************************
!  compute soltab_potsw (potential shortwave radiation)
!  and soltab_sunhrs (hours between sunrise and sunset)
!  for each HRU for each day of the year.
!***********************************************************************
      SUBROUTINE compute_soltab(Obliquity, Solar_declination, Slope, Aspect, &
     &                          Latitude, Cossl, Soltab, Sunhrs, Hru_type, Id)
      USE PRMS_SOLTAB, ONLY: PI, TWOPI, RADIANS, PI_12
      USE PRMS_BASIN, ONLY: DNEARZERO
      IMPLICIT NONE
      EXTERNAL compute_t
!     Functions
      DOUBLE PRECISION, EXTERNAL :: func3
      INTRINSIC ASIN, SIN, COS, ATAN, ABS
!     Arguments
      INTEGER, INTENT(IN) :: Hru_type, Id
      DOUBLE PRECISION, INTENT(IN), DIMENSION(366) :: Obliquity, Solar_declination
      REAL, INTENT(IN) :: Slope, Aspect, Latitude
      DOUBLE PRECISION, INTENT(OUT) :: Cossl
      DOUBLE PRECISION, INTENT(OUT), DIMENSION(366) :: Soltab, Sunhrs
!     Local Variables
      INTEGER :: jd
      DOUBLE PRECISION :: a, x0, x1, x2, r0, r1, d1, t, sunh, solt
      DOUBLE PRECISION :: t0, t1, t2, t3, t6, t7, d, sl
!***********************************************************************
! from SWIFT (1976)
! x0, x1, x2 = l0, l1, l2
! sl = i

      sl = ATAN(Slope)
      Cossl = COS(sl)
      a = Aspect*RADIANS

! x0 latitude of HRU
      x0 = Latitude*RADIANS

! x1 latitude of equivalent slope
! This is equation 13 from Lee, 1963
      x1 = ASIN(Cossl*SIN(x0)+SIN(sl)*COS(x0)*COS(a))

! d1 is the denominator of equation 12, Lee, 1963
      d1 = Cossl*COS(x0) - SIN(sl)*SIN(x0)*COS(a)
      IF ( ABS(d1)<DNEARZERO ) d1 = DNEARZERO

! x2 is the difference in longitude between the location of
! the HRU and the equivalent horizontal surface expressed in angle hour
! This is equation 12 from Lee, 1963
      x2 = ATAN(SIN(sl)*SIN(a)/d1)
      IF ( d1<0.0D0 ) x2 = x2 + PI

! r0 is the minute solar constant cal/cm2/min
      r0 = 2.0D0
! r0 could be 1.95 (Drummond, et al 1968)
      DO jd = 1, 366
        d = Solar_declination(jd)

! This is adjusted to express the variability of available insolation as
! a function of the earth-sun distance.  Lee, 1963, p 16.
! r1 is the hour solar constant cal/cm2/hour
! r0 is the minute solar constant cal/cm2/min
! 60.0D0 is minutes in an hour
! Obliquity is the obliquity of the ellipse of the earth's orbit around the sun. E
! is also called the radius vector of the sun (or earth) and is the ratio of
! the earth-sun distance on a day to the mean earth-sun distance.
! obliquity = ~23.439 (obliquity of sun)
        r1 = 60.0D0*r0/(Obliquity(jd)*Obliquity(jd))

!  compute_t is the sunrise equation.
!  t7 is the hour angle of sunset on the equivalent slope
!  t6 is the hour angle of sunrise on the equivalent slope
        CALL compute_t(x1, d, t)
        t7 = t - x2
        t6 = -t - x2

!  compute_t is the sunrise equation.
!  t1 is the hour angle of sunset on a hroizontal surface at the HRU
!  t0 is the hour angle of sunrise on a hroizontal surface at the HRU
        CALL compute_t(x0, d, t)
        t1 = t
        t0 = -t

! For HRUs that have an east or west direction component to their aspect, the
! longitude adjustment (moving the effective slope east or west) will cause either:
! (1) sunrise to be earlier than at the horizontal plane at the HRU
! (2) sunset to be later than at the horizontal plane at the HRU
! This is not possible. The if statements below check for this and adjust the
! sunrise/sunset angle hours on the equivalent slopes as necessary.
!
! t3 is the hour angle of sunrise on the slope at the HRU
! t2 is the hour angle of sunset on the slope at the HRU
        IF ( t7>t1 ) THEN
          t3 = t1
        ELSE
          t3 = t7
        ENDIF
        IF ( t6<t0 ) THEN
          t2 = t0
        ELSE
          t2 = t6
        ENDIF

        IF ( ABS(sl)<DNEARZERO ) THEN
!  solt is Swift's R4 (potential solar radiation on a sloping surface cal/cm2/day)
!  Swift, 1976, equation 6
          solt = func3(0.0D0, x0, t1, t0, r1, d)
!  sunh is the number of hours of direct sunlight (sunset minus sunrise) converted
!  from angle hours in radians to hours (24 hours in a day divided by 2 pi radians
!  in a day).
          sunh = (t1-t0)*PI_12
        ELSE
          IF ( t3<t2 ) THEN
            t2 = 0.0D0
            t3 = 0.0D0
          ENDIF
          t6 = t6 + TWOPI
          IF ( t6<t1 ) THEN
            solt = func3(x2, x1, t3, t2, r1, d) + func3(x2, x1, t1, t6, r1, d)
            sunh = (t3-t2+t1-t6)*PI_12
          ELSE
            t7 = t7 - TWOPI
            IF ( t7>t0 ) THEN
              solt = func3(x2, x1, t3, t2, r1, d) + func3(x2, x1, t7, t0, r1, d)
              sunh = (t3-t2+t7-t0)*PI_12
            ELSE
              solt = func3(x2, x1, t3, t2, r1, d)
              sunh = (t3-t2)*PI_12
            ENDIF
          ENDIF
        ENDIF
        IF ( solt<0.0D0 ) THEN
          PRINT *, 'WARNING: solar table value for day:', jd, &
     &             ' computed as:', solt, ' set to', 0.0, &
     &             ' for HRU:', Id, ' hru_type:', Hru_type
          PRINT *, 'slope, aspect, latitude, cossl', Slope, Aspect, Latitude, Cossl
          solt = 0.0D0
          PRINT *, Slope, Aspect, Latitude, Cossl, sunh
          PRINT *, t0, t1, t2, t3, t6, t7, d
        ENDIF
        IF ( sunh<DNEARZERO ) sunh = 0.0D0
        Sunhrs(jd) = sunh
        Soltab(jd) = solt

      ENDDO

      END SUBROUTINE compute_soltab

!***********************************************************************
!***********************************************************************
      SUBROUTINE compute_t(Lat, Solar_declination, T)
      USE PRMS_SOLTAB, ONLY: PI
      IMPLICIT NONE
      INTRINSIC TAN, ACOS
! Arguments
      DOUBLE PRECISION, INTENT(IN) :: Lat, Solar_declination
      DOUBLE PRECISION, INTENT(OUT) :: T
! Local Variables
      DOUBLE PRECISION :: tx
!***********************************************************************

!  This is the sunrise equation
!  Lat is the latitude
!  Solar_declination is the declination of the sun on a day
!  T is the angle hour from the local meridian (local solar noon) to the
!  sunrise (negative) or sunset (positive).  The Earth rotates at the angular
!  speed of 15 degrees/hour (2 pi / 24 hour in radians) and, therefore, T/15 degress (T*24/pi
!  in radians) gives the time of sunrise as the number of hours before the local
!  noon, or the time of sunset as the number of hours after the local noon.
!  Here the term local noon indicates the local time when the sun is exactly to
!  the south or north or exactly overhead.
      tx = -TAN(Lat)*TAN(Solar_declination)
      IF ( tx<-1.0D0 ) THEN
        T = PI
!rsr bug fix, old code would set t=acos(0.0) for tx>1 12/05
      ELSEIF ( tx>1.0D0 ) THEN
        T = 0.0D0
      ELSE
        T = ACOS(tx)
      ENDIF

      END SUBROUTINE compute_t

!***********************************************************************
!***********************************************************************
      DOUBLE PRECISION FUNCTION func3(V, W, X, Y, R1, Solar_declination)
      USE PRMS_SOLTAB, ONLY: PI_12
      IMPLICIT NONE
      INTRINSIC SIN, COS
! Arguments
      DOUBLE PRECISION, INTENT(IN) :: V, W, X, Y, R1, Solar_declination
!***********************************************************************
!  This is the radian angle version of FUNC3 (eqn 6) from Swift, 1976
!  or Lee, 1963 equation 5.
!  func3 (R4) is potential solar radiation on the surface cal/cm2/day
!  V (L2) latitude angle hour offset between actual and equivalent slope
!  W (L1) latitude of the equivalent slope
!  X (T3) hour angle of sunset on equivalent slope
!  Y (T2) hour angle of sunrise on equivalent slope
!  R1 solar constant for 60 minutes
!  Solar_declination declination of sun
      func3 = R1*PI_12*(SIN(Solar_declination)*SIN(W)*(X-Y) + COS(Solar_declination)*COS(W)*(SIN(X+V)-SIN(Y+V)))

      END FUNCTION func3


*=======================================================================
* File:    masoqp.f
* Author:  Alex Stivala, based on spsolqp.m by Prof. Yinyu Ye.
* Created: July 2008
*
* This is a reimplementation using FORTRAN-77 and HSL MA57
* of spsolqp.m from Prof. Yinyu Ye
* http://www.stanford.edu/~yyye/matlab/spsolqp.m
*
* This uses sparse symmetric linear solver MA57 from HSL 2007
* http://hsl.rl.ac.uk/hsl2007/distrib/hsl2007.html
* needs registration as a registred researcher
*
* LAPACK and BLAS are also required.
*
* $Id: masoqp.F 2970 2009-11-22 01:47:55Z astivala $
*=======================================================================

      subroutine solvqp(Q, ldq, A, lda, m, n,  b, c, x, y, info)
!      use IFPORT
      implicit none
*
* solvqp - solve quadratic program in standard form
*
*  This program solves quadratic program in standard form:
*
*     minimize    0.5*(x'*Q*x)+c'*x
*     subject to  A*x=b, x>=0.
*
*     This program is the implementation of the interior ellipsoidal
*     trust region and barrier function algorithm with dual solution
*     updating technique in the standard QP form. Two phases are used:
*     the first uses MAPHS1 find an interior feasible point and the
*     second uses MAPHS2 to find a local optimal solution.
*
*  Technical Reference
*
*     Y. Ye, "An extension of Karmarkar's algorithm and the trust region
*     method for convex quadratic programming," in Progress in
*     Mathematical Programming (N. Megiddo ed.), Springer-Verlag, NY
*     (1989) 49-63.
*
*     Y. Ye, "On affine-scaling algorithms for nonconvex quadratic
*     programming," Math. Programming 56 (1992) 285-300.
*
* See also
*
*      Y. Ye.  Interior Point Algorithms: Theory and Analysis.
*      Wiley-Interscience 1997.
*
*  Comment: Each iteration we solve a linear KKT system like
*
*  ( Q+mu X^{-2}   A^T )(dx) = c'
*  (     A          0  )(dy) = 0
*
*  where X = diag(x)  which is a positive diagonal matrix.

*
*     .. Scalar Arguments ..
      integer            ldq,lda,m,n,info
*     ..
*     .. Array Arguments ..
      double precision   Q(ldq,*), A(lda,*), x(*), y(*), b(*), c(*)
*     ..
*

*
* Arguments
* =========
*
* Q      (input) DOUBLE PRECISION array, dimension (n, n)
*        the symmetric objective matrix
*
* ldq    (input) INTGER
*        leading dimension of Q. ldq >= n
*
* A      (input) DOUBLE PRECISION array, dimension (m, n)
*        the constraint left-hand matrix
*
* lda    (input) INTEGER
*        leading dimenion of A. lda >= m
*
* m      (input) INTEGER
*        number of rows in matrix A
*        m <= n
*
* n      (input) INTEGER
*        number of rows and columns in marix Q, also the
*        number of columns in matrix A
*
* b      (input) DOUBLE PRECISION vector, dimension(m)
*        constraint right-hand column vector
*
* c      (input) DOUBLE PRECISION vector, dimension(m)
*        objective column vector
*
* x      (output) DOUBLE PRECISION vector, dimension (n+1)
*        solution vector with objective value appended as last element
*
* y      (output) DOUBLE PRECISION vector, dimension (m)
*        optimal dual solution (Lagrangian multiplier)
*
* info   (output) INTEGER
*        on exit, status of the computation
*        =  0 : successful exit
*        = -1 : phase 1 failed
*        = -2 : sizes exceed maximum dimensions
*        = -3 : an MA57 routine failed
*        = -4 : phase 2 failed
*        = -5 : max number of iterations exceeded
*        = -6 : numer of nonzero entries exceeds maximum dimensioned for
*        =  1 : no feasible point
*        =  2 : problem appears unbounded
*
*=======================================================================
*

*     .. Parameters ..

*     relative stopping tolerance: the objective value close to the
*     local optimum in the range of tolerance.
      double precision toler
      parameter (toler = 1.0d-5)

*     step size 0 < beta < 1
      double precision beta
      parameter (beta = 0.8d0)

*     largest values of array dimensions m and n allowed
      integer mmax,nmax
      parameter(mmax = 220, nmax = 4000)

*     largest number of nonzero entries in sparse E matrix
      integer nzmax
      parameter(nzmax = 10000000)

*     MA57 workspace size
      integer           lkeep
      parameter         (lkeep = 5 *(mmax+nmax) + nzmax+nzmax+42)
      integer iworklen,lwork,lfact,lifact
      parameter(iworklen = 5*mmax*nmax)
      parameter(lwork = mmax*nmax)
      parameter(lfact = 5*mmax*nmax)
      parameter(lifact = 5*mmax*nmax)

*     limit on number of iterations
      integer maxiter
      parameter (maxiter = 1000)

*     -inf value (may be system dependent)
      double precision neginf
      parameter (neginf=-1.79769d+308)
*     ..
*     .. Common block Arrays ..
*     lsyfac2 is true after maphs2 symbolic factorization done
*     lsyfac1 is true after maphs1 symbolic factorization done
      logical lsyfac2,lsyfac1
      common /macommon/ lsyfac2,lsyfac1
*     ..
*     .. Local Scalars ..
      character trans,uplo
      integer i,j,incr,order,iter,ldf,ldxx,ldaa,
     $     ixE
*     MA57 variables
      integer ne,job
*     other local scalars
      double precision alpha,gap,ob,z,blasa,blasb,lambda,obvalue,
     $     nora,lower,xdotv,cdotx
*     ..
*     .. Local Arrays ..
*     MA57 workspace
      integer iwork(iworklen)
      integer icntl(20),mainfo(40)
      double precision cntl(5),rinfo(20)
      double precision work(lwork)
      integer ifact(lifact)
      double precision fact(lfact)
      integer keep(lkeep)
*     others
      double precision avec(mmax),zhis(maxiter),comp(mmax+nmax)
*     sparse storage for matrix E for MA57 in HSL sparse symmetric format
      integer Eirn(nzmax),Ejcn(nzmax)
      double precision Ea(nzmax)
*     workspace arrays for MAPHS1
*     F (each column of F separately), dx, ipiv,v reused for MAPHS2
*     v,ipiv,E and F are also used as working storage in MASOQP itself
      double precision dx(nmax), F(mmax+nmax, 2)
      double precision  u(nmax+2), v(nmax+2),
     $     umzv(nmax+2), obz(nmax+2), y1(mmax+nmax), y2(mmax+nmax)
*     additional workspace arrays for MAPHS2
      double precision XX(nmax,nmax),AA(mmax,nmax)
*     ..
*     .. Intrinsic Functions ..
      intrinsic min,abs,max,dsqrt,dble
#if defined(__INTEL_COMPILER) || defined(__PORTLAND_COMPILER)
      intrinsic RANDOM_NUMBER
#else
      intrinsic rand,srand
#endif
*     ..
*     .. External Subroutines and Functions ..
      external maphs1,maphs2,drecip,demvv
      double precision dvmin,dvmax
      external dvmin,dvmax
      external rand55
*     MA57 subroutines
      external MA57ID,MA57BD,MA57CD
*     BLAS level 1 subroutines/functions
      external dcopy,dscal,daxpy
      double precision ddot
      external ddot
*     BLAS level 2 subroutines
      external dgemv,dsymv
*     ..
*     .. Executable Statements ..
*

      if  (m .gt. mmax) then
         write(*,*) 'maximum dimensions exceeded, increase mmax'
         info = -2
         return
      endif
      if  (n .gt. nmax) then
         write(*,*) 'maximum dimensions exceeded, increase nmax'
         info = -2
         return
      endif

      ldf = mmax+nmax
      ldxx = nmax
      ldaa = mmax

C      call srand(1)

*     set parameters for MA57
      call MA57ID(cntl, icntl)

      lsyfac2 = .false.
      lsyfac1 = .false.

* ----------------------------------------------------------------------
*
*     phase 1: try to find an interior feasible point
*
* ----------------------------------------------------------------------

      alpha = 0.95d0
      do 10 i = 1, n+1
         x(i) = 1.0d0
 10   continue
      ob = x(n+1)
      z = 0.0d0

*     avec = b - A*ones(n,1) (note using x as ones here)
      trans = 'N'
      blasa = -1.0d0
      blasb = 1.0d0
      incr = 1
      call dcopy(m, b, incr, avec, incr)
      call dgemv(trans, m, n, blasa, A, lda, x, incr, blasb,
     $     avec, incr)

      gap = ob - z
 500  if (gap .ge. toler) then
         call maphs1(A, lda, m, n, avec, b, x, y, z, ob, alpha,
     $        dx, Eirn, Ejcn, Ea, lkeep, keep,
     $        nzmax, F, ldf, u, v, umzv, obz,
     $        y1, y2, info)
         if (info .ne. 0) then
            write(*,*) 'phase 1 failed'
            info = -1
            return
         endif

         ob = x(n+1)
         gap = ob - z
         if (z .gt. 0.0d0) then
            gap  = -1.0d0
            write(*,*) 'The system has no feasible solution.'
            info = 1
            return
         endif
         goto 500
      endif

* ----------------------------------------------------------------------
*
*     phase 2: search for an optimal solution
*
* ----------------------------------------------------------------------

*     only using first n elements of x from now on

*     always using increment of 1 in BLAS calls
      incr = 1

      alpha =  0.9d0

*     ==================================================================
*
*     Solve the linear system
*
*     E * comp = y1 where
*
*     E (m+n, m+n)  =    ( eye (n, n)   A' (n, m) )
*                        ( A  (m, n)    0  (m, m) )
*
*     y1 (m+n)      =    ( r (n, 1) )
*                        ( 0 (m, 1) )
*
*     where r is an n-vector of psuedorandom number uniformly
*     distributed in [0,1].
*
*     E is square, symmetric, and sparse. But it is not necessarily
*     positive definite so we cannot always use CHOLMOD to efficiently
*     solve the system. We therefore use MA57 with sparse matrix E
*     stored in the HSL2007 MA57 format.  In this format, the i and j
*     indices of the nonzero entries of E are stored in Eirn and Ejcn
*     respectively. (Only one triangle is stored). The nonzero values
*     are stored in Ea, corresponding to the i,j indices in Eirn and
*     Ejcn. Only the upper triangle is stored.
*
*     If E were positive definite, which can efficiently be tested
*     for with Cholesky factorization, it could be solved by that means
*     more efficiently than the general case using LU decomposition.
*
*     TODO: use CHOLMOD to test for postive definiteness and use
*           Cholesky factorization if it is.
*
*     ==================================================================

*     ixE is the current index in Eirn and Ejcn and Ea
      ixE = 1
      do 730 i = 1, n
         Eirn(ixE) = i
         Ejcn(ixE) = j
         Ea(ixE) = 1.0
         ixE = ixE + 1
         do 720 j = 1,m
            if (A(j,i) .ne. 0.0d0) then
               Eirn(ixE) = i
               Ejcn(ixE) = n + j
               Ea(ixE) = A(j,i)
               ixE = ixE +1
               if (ixE .gt. nzmax) then
                  write(*,*) 'Max number of nonzero values exceeded.'
                  info = -6
                  return
               endif
            endif
 720     continue
 730  continue
      ne = ixE - 1

*     use y1  as RHS for linear system
      do 800 j = 1, n
#if defined(__INTEL_COMPILER) || defined(__PORTLAND_COMPILER)
         CALL RANDOM_NUMBER(y1(j))
#else
         y1(j) = dble(rand(0))
#endif
 800  continue
      do 810 j = n+1, n+m
         y1(j) = 0.0d0
 810  continue

      order = m + n

*     numeric factorization and
*     solve E*comp=y1 with iterative refinement
*     NB we are reusing the symbolic factorization done in maphs1
      CALL MA57BD (order, ne, Ea, fact, lfact, ifact, lifact,
     $     lkeep, keep, iwork,
     $     icntl, cntl, mainfo, rinfo)
      if (mainfo(1) .lt. 0) then
         write(*,*) 'MA57 factorization  failed, info = ',
     $        mainfo(1)
         info = -3
         return
      endif
      job = 0
      CALL MA57CD (job, order, fact, lfact, ifact, lifact,
     $     1, y1, mmax+nmax, work, lwork, iwork, icntl, mainfo)
      if (mainfo(1) .lt. 0) then
         write(*,*) 'MA57 solve failed, info = ', mainfo(1)
         info = -3
         return
      endif
      call dcopy(order, y1, incr, comp, incr)

*     nora = min(comp./x)
*     solution is in comp and using y2 as temp storage
      call dcopy(n, x, incr, y2, incr)
      call drecip(n, y2)
      call demvv(n, comp, y2)
      nora = dvmin(n, y2)

      if (nora .le. 0.0d0) then
         nora = -0.01d0 / nora
      else
         nora = dvmax(n, y2)
         if (nora .eq. 0.0d0) then
            write(*,*) 'The problem has a unique feasible point.'
            x(n+1) = ob
            info = 0
            return
         endif
         nora = 0.01d0 / nora
      endif
*     x = x + nora*comp
      call daxpy(n, nora, comp, incr, x, incr)
*     obvalue = (x'*(Q*x))/2 + c'*x
      uplo = 'U'
      blasa = 1.0d0
      blasb = 0.0d0
*     blasb is zero so v can be uninitialized on entry to DSYMV
      call dsymv(uplo, n, blasa, Q, ldq, x, incr, blasb, v, incr)
C      obvalue = ddot(n, x, incr, v, incr) / 2.0d0 +
C     $          ddot(n, c, incr, x, incr)
      xdotv = ddot(n, x, incr, v, incr)
      cdotx = ddot(n, c, incr, x, incr)
      obvalue = xdotv / 2.0d0 + cdotx

      lower = neginf
      zhis(1) = lower
      gap = 1.0d0
      lambda = max(1.0d0, abs(obvalue)/dsqrt(dsqrt(dble(n))))
      iter = 0
 900  if (gap .ge. toler) then
         iter = iter + 1
         if (iter .ge. maxiter) then
            write(*,*) 'Maximum number of iterations exceeded.'
            x(n+1) = ob
            info = -5
            return
         endif

         call maphs2(Q, ldq, A, lda, m, n, c,
     $        alpha, beta, lambda, toler,
     $        obvalue, ob, x, y, Eirn, Ejcn, Ea,
     $        nzmax, lkeep, keep,
     $        y2, y1, dx, F,
     $        XX, ldxx, AA, ldaa,
     $        v, info)

         if (info .ne. 0) then
            write(*,*) 'phase 2 failed'
            info = -4
            return
         endif

C          write(*,910),iter,ob
C  910     format('iter = ',i5,' ob = ',f8.4)

         if (ob .eq. neginf) then
            gap = 0.0d0
            write(*,*) 'The problem is unbounded.'
            info = 2
            return
         else
*           comp = Q*x + c - A'*y
*           first compute comp <- Q*x + c, then comp <- -A'*y + comp
            uplo = 'U'
            blasa = 1.0d0
            blasb = 1.0d0
            call dcopy(n, c, incr, comp, incr)
            call dsymv(uplo,n,blasa,Q,ldq,x,incr,blasb,comp,incr)
            trans = 'T'
            blasa = -1.0d0
            blasb = 1.0d0
            call dgemv(trans, m, n, blasa, A, lda, y, incr, blasb,
     $           comp, incr)

            if (dvmin(n, comp) .ge. 0.0d0) then
               zhis(iter+1) = ob - ddot(n, x, incr, comp, incr)
               lower = zhis(iter+1)
               gap = (ob-lower)/(1.0d0+abs(ob))
               obvalue = ob
            else
               zhis(iter+1) = zhis(iter)
               lower = zhis(iter+1)
               gap = (obvalue - ob) / (1.0d0 + abs(ob))
               obvalue = ob
            endif
         endif
         goto 900
*     end of while (gap .ge. toler) loop
*     a (local) optimal solution has been found
      endif

      x(n+1) = ob
      info = 0

*     end of MASOQP
      return
      end


C$Procedure      CONICS ( Determine state from conic elements )

      SUBROUTINE CONICS ( ELTS, ET, STATE )

C$ Abstract
C
C     Determine the state (position, velocity) of an orbiting body
C     from a set of elliptic, hyperbolic, or parabolic orbital
C     elements.
C
C$ Disclaimer
C
C     THIS SOFTWARE AND ANY RELATED MATERIALS WERE CREATED BY THE
C     CALIFORNIA INSTITUTE OF TECHNOLOGY (CALTECH) UNDER A U.S.
C     GOVERNMENT CONTRACT WITH THE NATIONAL AERONAUTICS AND SPACE
C     ADMINISTRATION (NASA). THE SOFTWARE IS TECHNOLOGY AND SOFTWARE
C     PUBLICLY AVAILABLE UNDER U.S. EXPORT LAWS AND IS PROVIDED "AS-IS"
C     TO THE RECIPIENT WITHOUT WARRANTY OF ANY KIND, INCLUDING ANY
C     WARRANTIES OF PERFORMANCE OR MERCHANTABILITY OR FITNESS FOR A
C     PARTICULAR USE OR PURPOSE (AS SET FORTH IN UNITED STATES UCC
C     SECTIONS 2312-2313) OR FOR ANY PURPOSE WHATSOEVER, FOR THE
C     SOFTWARE AND RELATED MATERIALS, HOWEVER USED.
C
C     IN NO EVENT SHALL CALTECH, ITS JET PROPULSION LABORATORY, OR NASA
C     BE LIABLE FOR ANY DAMAGES AND/OR COSTS, INCLUDING, BUT NOT
C     LIMITED TO, INCIDENTAL OR CONSEQUENTIAL DAMAGES OF ANY KIND,
C     INCLUDING ECONOMIC DAMAGE OR INJURY TO PROPERTY AND LOST PROFITS,
C     REGARDLESS OF WHETHER CALTECH, JPL, OR NASA BE ADVISED, HAVE
C     REASON TO KNOW, OR, IN FACT, SHALL KNOW OF THE POSSIBILITY.
C
C     RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF
C     THE SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY
C     CALTECH AND NASA FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE
C     ACTIONS OF RECIPIENT IN THE USE OF THE SOFTWARE.
C
C$ Required_Reading
C
C     None.
C
C$ Keywords
C
C     CONIC
C     EPHEMERIS
C
C$ Declarations

      DOUBLE PRECISION      ELTS    ( 8 )
      DOUBLE PRECISION      ET
      DOUBLE PRECISION      STATE   ( 6 )

C$ Brief_I/O
C
C     VARIABLE  I/O  DESCRIPTION
C     --------  ---  --------------------------------------------------
C     ELTS       I   Conic elements.
C     ET         I   Input time.
C     STATE      O   State of orbiting body at ET.
C
C$ Detailed_Input
C
C     ELTS       are conic elements describing the orbit of a body
C                around a primary. The elements are, in order:
C
C                      RP      Perifocal distance.
C                      ECC     Eccentricity.
C                      INC     Inclination.
C                      LNODE   Longitude of the ascending node.
C                      ARGP    Argument of periapse.
C                      M0      Mean anomaly at epoch.
C                      T0      Epoch.
C                      MU      Gravitational parameter.
C
C                Units are km, rad, rad/sec, km**3/sec**2.  The epoch
C                is given in ephemeris seconds past J2000. The same
C                elements are used to describe all three types
C                (elliptic, hyperbolic, and parabolic) of conic orbit.
C
C     ET         is the time at which the state of the orbiting body
C                is to be determined, in ephemeris seconds J2000.
C
C$ Detailed_Output
C
C     STATE      is the state (position and velocity) of the body at
C                time ET. Components are x, y, z, dx/dt, dy/dt, dz/dt.
C
C$ Parameters
C
C      None.
C
C$ Exceptions
C
C     1) If the eccentricity supplied is less than 0, the error
C        'SPICE(BADECCENTRICITY)' is signalled.
C
C     2) If a non-positive periapse distance is supplied, the error
C       'SPICE(BADPERIAPSEVALUE)' is signalled.
C
C     3) If a non-positive value for the attracting mass is supplied,
C        the error 'SPICE(BADGM)',  is signalled.
C
C     4) Errors such as an out of bounds value for ET are diagnosed
C        by routines called by this routine.
C
C$ Files
C
C     None.
C
C$ Particulars
C
C     None.
C
C$ Examples
C
C     Let VINIT contain the initial state of a spacecraft relative to
C     the center of a planet at epoch ET, and let GM be the gravitation
C     parameter of the planet. The call
C
C        CALL OSCELT ( VINIT, ET, GM, ELTS )
C
C     produces a set of osculating elements describing the nominal
C     orbit that the spacecraft would follow in the absence of all
C     other bodies in the solar system and non-gravitational forces
C     on the spacecraft.
C
C     Now let STATE contain the state of the same spacecraft at some
C     other epoch, LATER. The difference between this state and the
C     state predicted by the nominal orbit at the same epoch can be
C     computed as follows.
C
C        CALL CONICS ( ELTS, LATER, NOMINAL )
C        CALL VSUBG  ( NOMINAL, STATE, 6, DIFF )
C
C        WRITE (*,*) 'Perturbation in x, dx/dt = ', DIFF(1), DIFF(4)
C        WRITE (*,*) '                y, dy/dt = ', DIFF(2), DIFF(5)
C        WRITE (*,*) '                z, dz/dt = ', DIFF(3), DIFF(6)
C
C$ Restrictions
C
C     None.
C
C$ Literature_References
C
C     [1] Roger Bate, Fundamentals of Astrodynamics, Dover, 1971.
C
C$ Author_and_Institution
C
C     I.M. Underwood  (JPL)
C     W.L. Taber      (JPL)
C
C$ Version
C
C-    SPICELIB Version 4.0.0, 26-MAR-1998 (WLT)
C
C        There was a coding error in the computation of the mean
C        anomaly in the parabolic case.  This problem has been
C        corrected.
C
C-    SPICELIB Version 3.0.1, 15-OCT-1996 (WLT)
C
C        Corrected a typo in the description of the units associated
C        with the input elements.
C
C-    SPICELIB Version 3.0.0, 12-NOV-1992 (WLT)
C
C        The routine was re-written to make use of NAIF's universal
C        variables formulation for state propagation (PROP2B).  As
C        a result, several problems were simultaneously corrected.
C
C        A major bug was fixed that caused improper state evaluations
C        for ET's that precede the epoch of the elements in the
C        elliptic case.
C
C        A danger of non-convergence in the solution of Kepler's
C        equation has been eliminated.
C
C        In addition to this reformulation of CONICS checks were
C        installed that ensure the elements supplied are physically
C        meaningful.  Eccentricity must be non-negative. The
C        distance at periapse and central mass must be positive.  If
C        not errors are signalled.
C
C-    SPICELIB Version 2.0.1, 10-MAR-1992 (WLT)
C
C        Comment section for permuted index source lines was added
C        following the header.
C
C-    SPICELIB Version 2.0.0, 19-APR-1991 (WLT)
C
C        An error in the hyperbolic state generation was corrected.
C
C-    SPICELIB Version 1.0.0, 31-JAN-1990 (IMU)
C
C-&

C$ Index_Entries
C
C     state from conic elements
C
C-&

C$ Revisions
C
C-    SPICELIB Version 3.0.1, 15-OCT-1996 (WLT)
C
C        Corrected a typo in the description of the units associated
C        with the input elements.
C
C-    SPICELIB Version 3.0.0, 12-NOV-1992 (WLT)
C
C        The routine was re-written to make use of NAIF's universal
C        variables formulation for state propagation (PROP2B).  As
C        a result, several problems were simultaneously corrected.
C
C        A major bug was fixed that caused improper state evaluations
C        for ET's that precede the epoch of the elements in the
C        elliptic case.
C
C        A danger of non-convergence in the solution of Kepler's
C        equation has been eliminated.
C
C        In addition to this reformulation of CONICS checks were
C        installed that ensure the elements supplied are physically
C        meaningful.  Eccentricity must be non-negative. The
C        distance at periapse and central mass must be positive.  If
C        not errors are signalled.
C
C        These changes were prompted by the discovery that the old
C        formulation had a severe bug for elliptic orbits and epochs
C        prior to the epoch of the input elements, and by the discovery
C        that the time of flight routines had problems with convergence.
C
C-    SPICELIB Version 2.0.0, 19-APR-1991 (WLT)
C
C        The original version of the routine had a bug in that
C        it attempted to restrict the hyperbolic anomaly to
C        the interval 0 to 2*PI.  This has been fixed.
C
C-    Beta Version 1.0.1, 27-JAN-1989 (IMU)
C
C        Examples section completed.
C
C-&

C
C     SPICELIB functions
C
      LOGICAL               RETURN
      DOUBLE PRECISION      TWOPI

C
C     Local variables
C
      DOUBLE PRECISION      RP
      DOUBLE PRECISION      ECC
      DOUBLE PRECISION      INC
      DOUBLE PRECISION      LNODE
      DOUBLE PRECISION      ARGP
      DOUBLE PRECISION      M0
      DOUBLE PRECISION      T0
      DOUBLE PRECISION      MU
      DOUBLE PRECISION      N
      DOUBLE PRECISION      PERIOD

      DOUBLE PRECISION      AINVRS
      DOUBLE PRECISION      DT

      DOUBLE PRECISION      COSI
      DOUBLE PRECISION      SINI
      DOUBLE PRECISION      COSN
      DOUBLE PRECISION      SINN
      DOUBLE PRECISION      COSW
      DOUBLE PRECISION      SINW

      DOUBLE PRECISION      SNCI
      DOUBLE PRECISION      CNCI

      DOUBLE PRECISION      BASISP     ( 3 )
      DOUBLE PRECISION      BASISQ     ( 3 )
      DOUBLE PRECISION      PSTATE     ( 6 )
      DOUBLE PRECISION      V

C
C      The only real work required by this routine is the construction
C      of a preliminary state vector from the input elements.  Once this
C      is in hand, we can simply let the routine PROP2B do the real
C      work, free from the instabilities inherent in the classical
C      elements formulation of two-body motion.
C
C      To do this we shall construct a basis of vectors that lie in the
C      plane of the orbit.  The first vector P shall point towards the
C      position of the orbiting body at periapse.  The second
C      vector Q shall point along the velocity vector of the body at
C      periapse.
C
C      The only other consideration is determining an epoch, TP, of
C      this state and the delta time ET - TP.
C

C
C     Standard SPICE error handling.
C
      IF ( RETURN () ) THEN
         RETURN
      ELSE
         CALL CHKIN ( 'CONICS' )
      END IF

C
C     Unpack the element vector.
C
      RP     = ELTS(1)
      ECC    = ELTS(2)
      INC    = ELTS(3)
      LNODE  = ELTS(4)
      ARGP   = ELTS(5)
      M0     = ELTS(6)
      T0     = ELTS(7)
      MU     = ELTS(8)

C
C     Handle all of the exceptions first.
C

      IF ( ECC .LT. 0 ) THEN
         CALL SETMSG ( 'The eccentricity supplied was negative. Only '//
     .                 'positive values are meaningful.  The value '  //
     .                 'was #'                  )
         CALL ERRDP  ( '#', ECC                 )
         CALL SIGERR ( 'SPICE(BADECCENTRICITY)' )
         CALL CHKOUT ( 'CONICS'                 )
         RETURN
      END IF

      IF ( RP .LE. 0 ) THEN
         CALL SETMSG ( 'The value of periapse range supplied was '    //
     .                 'non-positive.  Only positive values are '     //
     .                 'allowed.  The value supplied was #. '         )
         CALL ERRDP  ( '#', RP                                        )
         CALL SIGERR ( 'SPICE(BADPERIAPSEVALUE)'                      )
         CALL CHKOUT ( 'CONICS'                                       )
         RETURN
      END IF

      IF ( MU .LE. 0 ) THEN
         CALL SETMSG ( 'The value of GM supplied was '                //
     .                 'non-positive.  Only positive values are '     //
     .                 'allowed.  The value supplied was #. '         )
         CALL ERRDP  ( '#', MU                                        )
         CALL SIGERR ( 'SPICE(BADGM)'                                 )
         CALL CHKOUT ( 'CONICS' )
         RETURN
      END IF

C
C     First construct the orthonormal basis vectors that span the orbit
C     plane.
C
      COSI      =  DCOS ( INC   )
      SINI      =  DSIN ( INC   )
      COSN      =  DCOS ( LNODE )
      SINN      =  DSIN ( LNODE )
      COSW      =  DCOS ( ARGP  )
      SINW      =  DSIN ( ARGP  )

      SNCI      =  SINN * COSI
      CNCI      =  COSN * COSI

      BASISP(1) =  COSN * COSW  -  SNCI * SINW
      BASISP(2) =  SINN * COSW  +  CNCI * SINW
      BASISP(3) =  SINI * SINW

      BASISQ(1) = -COSN * SINW  -  SNCI * COSW
      BASISQ(2) = -SINN * SINW  +  CNCI * COSW
      BASISQ(3) =  SINI * COSW

C
C     Next construct the state at periapse.
C
C     The position at periapse is just BASISP scaled by the distance
C     at periapse.
C
C     The velocity must be constructed so that we can get an orbit
C     of this shape.  Recall that the magnitude of the specific angular
C     momentum vector is given by DSQRT ( MU*RP*(1+ECC) )
C     The velocity will be given by V * BASISQ.  But we must have the
C     magnitude of the cross product of position and velocity be
C     equal to DSQRT ( MU*RP*(1+ECC) ). So we must have
C
C        RP*V = DSQRT( MU*RP*(1+ECC) )
C
C     so that:
C
      V = DSQRT( MU*(1.0D0 + ECC) / RP )

      CALL VSCL( RP, BASISP, PSTATE(1) )
      CALL VSCL( V,  BASISQ, PSTATE(4) )

C
C     Finally compute DT the elapsed time since the epoch of periapse.
C     Ellipses first, since they are the most common.
C
      IF ( ECC .LT. 1.D0 ) THEN

C
C        Recall that:
C
C        N ( mean motion ) is given by DSQRT( MU / A**3 ).
C        But since, A = RP / ( 1 - ECC ) ...
C
         AINVRS = (1.0D0 - ECC)   / RP
         N      = SQRT(MU*AINVRS) * AINVRS
         PERIOD = TWOPI()         / N

C
C        In general the mean anomaly is given by
C
C           M  = (T - TP) * N
C
C        Where TP is the time of periapse passage.  M0 is the mean
C        anomaly at time T0 so that
C        Thus
C
C           M0 = ( T0 - TP ) * N
C
C        So TP = T0-M0/N hence the time since periapse at time ET
C        is given by ET - T0 + M0/N.  Finally, since elliptic orbits are
C        periodic, we can mod this value by the period of the orbit.
C
         DT = MOD ( ET - T0 + M0/N, PERIOD )

C
C     Hyperbolas next.
C
      ELSE IF ( ECC .GT. 1 ) THEN

C
C        Again, recall that:
C
C        N ( mean motion ) is given by DSQRT( MU / |A**3| ).
C        But since, |A| = RP / ( ECC - 1 ) ...
C
         AINVRS = (ECC - 1.0D0)    / RP
         N      = DSQRT(MU*AINVRS) * AINVRS
         DT     = ET - T0          + M0/N

C
C     Finally, parabolas.
C
      ELSE

         N  = DSQRT( MU / (2.0D0*RP) ) / RP
         DT = ET - T0                + M0/N

      END IF

C
C     Now let PROP2B do the work of propagating the state.
C
      CALL PROP2B ( MU, PSTATE, DT, STATE )

      CALL CHKOUT ( 'CONICS' )
      RETURN
      END


      function diskdf5ez(vr,vt,vz,r,z)

      real psirz,psir0

      psir0=pot(r,0.0)
      if (z.eq.0.) then
         psirz=psir0
      else
         psirz=pot(r,z)
      endif
c
      ep=0.5*(vr*vr+vt*vt)-psir0
c
      am=r*vt
c
      ez=0.5*vz*vz-psirz+psir0
c
      diskdf5ez=diskdf3ez(ep,am,ez)
      return
      end


!> \file sort.f90
!! \brief defines sorting routines
!! @author Innocent Souopgui
!!
!<
MODULE sort_tools
IMPLICIT NONE
  !> \brief sort routines, selection
  !<
  INTERFACE select_sort
     MODULE PROCEDURE select_sort_int
     !MODULE PROCEDURE select_sort_sp_real
     !MODULE PROCEDURE select_sort_dp_real
  END INTERFACE
  !> \brief switch routines
  !<
  INTERFACE switch
     MODULE PROCEDURE switch_int
     !MODULE PROCEDURE switch_sp_real
     !MODULE PROCEDURE switch_dp_real
  END INTERFACE
CONTAINS
  SUBROUTINE select_sort_int(ida_x)
    INTEGER, DIMENSION(:), INTENT(INOUT) :: ida_x
    INTEGER :: ibi, ibj, il_min_idx, il_size

    il_size = SIZE(ida_x)
    DO ibi = 1, il_size-1
      il_min_idx = ibi
      DO ibj = ibi+1, il_size
        IF( ida_x(ibj)<ida_x(il_min_idx) ) il_min_idx = ibj
      END DO
      IF(ibi /= il_min_idx) THEN
        CALL switch( ida_x(ibi), ida_x(il_min_idx) )
      END IF
    END DO
  END SUBROUTINE select_sort_int

  SUBROUTINE switch_int(id_a, id_b)
    INTEGER, INTENT(INOUT) :: id_a, id_b
    !local variables
    INTEGER :: il_tmp

    il_tmp = id_a
    id_a = id_b
    id_b = il_tmp
  END SUBROUTINE switch_int
END MODULE sort_tools


! =====================================================
subroutine rpn2(ixy,maxm,meqn,mwaves,maux,mbc,mx,ql,qr,auxl,auxr,wave,s,amdq,apdq)
! =====================================================
! Riemann solver for the non-conservative transport equation
!    q_t  +  u(x,y,t)*q_x + v(x,y,t)*q_y = 0
! where u and v are a given velocity field and q has multiple components.
! The velocities are specified at cell edges, so that

!       u(i,j) = u(x_(i-1/2), y_j)
!       v(i,j) = v(x_i, y_(j-1/2))

! The size of q should be given by meqn.

! waves: 1
! equations: meqn
! aux fields: 2

! Conserved quantities: meqn

! Auxiliary variables:
!         1  x_velocity (u)
!         2  y_velocity (v)

! solve Riemann problems along one slice of data.
! This data is along a slice in the x-direction if ixy=1
!                            or the y-direction if ixy=2.

! On input, ql contains the state vector at the left edge of each cell
!           qr contains the state vector at the right edge of each cell

! On output, wave contains the waves, s the speeds,
! and amdq, apdq the left-going and right-going flux differences,
! respectively.  Note that in this advective form, the sum of
! amdq and apdq is not equal to a difference of fluxes except in the
! case of constant velocities.

! Note that the i'th Riemann problem has left state qr(i-1,:)
!                                    and right state ql(i,:)
! From the basic clawpack routines, this routine is called with ql = qr

    implicit real*8(a-h,o-z)

    dimension wave(meqn, mwaves, 1-mbc:maxm+mbc)
    dimension    s(mwaves, 1-mbc:maxm+mbc)
    dimension   ql(meqn, 1-mbc:maxm+mbc)
    dimension   qr(meqn, 1-mbc:maxm+mbc)
    dimension apdq(meqn,1-mbc:maxm+mbc)
    dimension amdq(meqn,1-mbc:maxm+mbc)
    dimension auxl(maux,1-mbc:maxm+mbc)
    dimension auxr(maux,1-mbc:maxm+mbc)

    do i = 2-mbc, mx+mbc
        wave(:,1,i) = ql(:,i) - qr(:,i-1)
        s(1,i) = auxl(ixy,i)
        ! The flux difference df = s*wave  all goes in the downwind direction:
        amdq(:,i) = dmin1(auxl(ixy,i), 0.d0) * wave(:,1,i)
        apdq(:,i) = dmax1(auxl(ixy,i), 0.d0) * wave(:,1,i)
    end do

    return
    end subroutine rpn2


!*==cgbbrd.f90  processed by SPAG 7.51RB at 20:08 on  3 Mar 2022
!> \brief \b CGBBRD
!
!  =========== DOCUMENTATION ===========
!
! Online html documentation available at
!            http://www.netlib.org/lapack/explore-html/
!
!> \htmlonly
!> Download CGBBRD + dependencies
!>
!> [TGZ]
!>
!> [ZIP]
!>
!> [TXT]
!> \endhtmlonly
!
!  Definition:
!  ===========
!
!       SUBROUTINE CGBBRD( VECT, M, N, NCC, KL, KU, AB, LDAB, D, E, Q,
!                          LDQ, PT, LDPT, C, LDC, WORK, RWORK, INFO )
!
!       .. Scalar Arguments ..
!       CHARACTER          VECT
!       INTEGER            INFO, KL, KU, LDAB, LDC, LDPT, LDQ, M, N, NCC
!       ..
!       .. Array Arguments ..
!       REAL               D( * ), E( * ), RWORK( * )
!       COMPLEX            AB( LDAB, * ), C( LDC, * ), PT( LDPT, * ),
!      $                   Q( LDQ, * ), WORK( * )
!       ..
!
!
!> \par Purpose:
!  =============
!>
!> \verbatim
!>
!> CGBBRD reduces a complex general m-by-n band matrix A to real upper
!> bidiagonal form B by a unitary transformation: Q**H * A * P = B.
!>
!> The routine computes B, and optionally forms Q or P**H, or computes
!> Q**H*C for a given matrix C.
!> \endverbatim
!
!  Arguments:
!  ==========
!
!> \param[in] VECT
!> \verbatim
!>          VECT is CHARACTER*1
!>          Specifies whether or not the matrices Q and P**H are to be
!>          formed.
!>          = 'N': do not form Q or P**H;
!>          = 'Q': form Q only;
!>          = 'P': form P**H only;
!>          = 'B': form both.
!> \endverbatim
!>
!> \param[in] M
!> \verbatim
!>          M is INTEGER
!>          The number of rows of the matrix A.  M >= 0.
!> \endverbatim
!>
!> \param[in] N
!> \verbatim
!>          N is INTEGER
!>          The number of columns of the matrix A.  N >= 0.
!> \endverbatim
!>
!> \param[in] NCC
!> \verbatim
!>          NCC is INTEGER
!>          The number of columns of the matrix C.  NCC >= 0.
!> \endverbatim
!>
!> \param[in] KL
!> \verbatim
!>          KL is INTEGER
!>          The number of subdiagonals of the matrix A. KL >= 0.
!> \endverbatim
!>
!> \param[in] KU
!> \verbatim
!>          KU is INTEGER
!>          The number of superdiagonals of the matrix A. KU >= 0.
!> \endverbatim
!>
!> \param[in,out] AB
!> \verbatim
!>          AB is COMPLEX array, dimension (LDAB,N)
!>          On entry, the m-by-n band matrix A, stored in rows 1 to
!>          KL+KU+1. The j-th column of A is stored in the j-th column of
!>          the array AB as follows:
!>          AB(ku+1+i-j,j) = A(i,j) for max(1,j-ku)<=i<=min(m,j+kl).
!>          On exit, A is overwritten by values generated during the
!>          reduction.
!> \endverbatim
!>
!> \param[in] LDAB
!> \verbatim
!>          LDAB is INTEGER
!>          The leading dimension of the array A. LDAB >= KL+KU+1.
!> \endverbatim
!>
!> \param[out] D
!> \verbatim
!>          D is REAL array, dimension (min(M,N))
!>          The diagonal elements of the bidiagonal matrix B.
!> \endverbatim
!>
!> \param[out] E
!> \verbatim
!>          E is REAL array, dimension (min(M,N)-1)
!>          The superdiagonal elements of the bidiagonal matrix B.
!> \endverbatim
!>
!> \param[out] Q
!> \verbatim
!>          Q is COMPLEX array, dimension (LDQ,M)
!>          If VECT = 'Q' or 'B', the m-by-m unitary matrix Q.
!>          If VECT = 'N' or 'P', the array Q is not referenced.
!> \endverbatim
!>
!> \param[in] LDQ
!> \verbatim
!>          LDQ is INTEGER
!>          The leading dimension of the array Q.
!>          LDQ >= max(1,M) if VECT = 'Q' or 'B'; LDQ >= 1 otherwise.
!> \endverbatim
!>
!> \param[out] PT
!> \verbatim
!>          PT is COMPLEX array, dimension (LDPT,N)
!>          If VECT = 'P' or 'B', the n-by-n unitary matrix P'.
!>          If VECT = 'N' or 'Q', the array PT is not referenced.
!> \endverbatim
!>
!> \param[in] LDPT
!> \verbatim
!>          LDPT is INTEGER
!>          The leading dimension of the array PT.
!>          LDPT >= max(1,N) if VECT = 'P' or 'B'; LDPT >= 1 otherwise.
!> \endverbatim
!>
!> \param[in,out] C
!> \verbatim
!>          C is COMPLEX array, dimension (LDC,NCC)
!>          On entry, an m-by-ncc matrix C.
!>          On exit, C is overwritten by Q**H*C.
!>          C is not referenced if NCC = 0.
!> \endverbatim
!>
!> \param[in] LDC
!> \verbatim
!>          LDC is INTEGER
!>          The leading dimension of the array C.
!>          LDC >= max(1,M) if NCC > 0; LDC >= 1 if NCC = 0.
!> \endverbatim
!>
!> \param[out] WORK
!> \verbatim
!>          WORK is COMPLEX array, dimension (max(M,N))
!> \endverbatim
!>
!> \param[out] RWORK
!> \verbatim
!>          RWORK is REAL array, dimension (max(M,N))
!> \endverbatim
!>
!> \param[out] INFO
!> \verbatim
!>          INFO is INTEGER
!>          = 0:  successful exit.
!>          < 0:  if INFO = -i, the i-th argument had an illegal value.
!> \endverbatim
!
!  Authors:
!  ========
!
!> \author Univ. of Tennessee
!> \author Univ. of California Berkeley
!> \author Univ. of Colorado Denver
!> \author NAG Ltd.
!
!> \date December 2016
!
!> \ingroup complexGBcomputational
!
!  =====================================================================
      SUBROUTINE CGBBRD(Vect,M,N,Ncc,Kl,Ku,Ab,Ldab,D,E,Q,Ldq,Pt,Ldpt,C, &
     &                  Ldc,Work,Rwork,Info)
      IMPLICIT NONE
!*--CGBBRD197
!
!  -- LAPACK computational routine (version 3.7.0) --
!  -- LAPACK is a software package provided by Univ. of Tennessee,    --
!  -- Univ. of California Berkeley, Univ. of Colorado Denver and NAG Ltd..--
!     December 2016
!
!     .. Scalar Arguments ..
      CHARACTER Vect
      INTEGER Info , Kl , Ku , Ldab , Ldc , Ldpt , Ldq , M , N , Ncc
!     ..
!     .. Array Arguments ..
      REAL D(*) , E(*) , Rwork(*)
      COMPLEX Ab(Ldab,*) , C(Ldc,*) , Pt(Ldpt,*) , Q(Ldq,*) , Work(*)
!     ..
!
!  =====================================================================
!
!     .. Parameters ..
      REAL ZERO
      PARAMETER (ZERO=0.0E+0)
      COMPLEX CZERO , CONE
      PARAMETER (CZERO=(0.0E+0,0.0E+0),CONE=(1.0E+0,0.0E+0))
!     ..
!     .. Local Scalars ..
      LOGICAL wantb , wantc , wantpt , wantq
      INTEGER i , inca , j , j1 , j2 , kb , kb1 , kk , klm , klu1 ,     &
     &        kun , l , minmn , ml , ml0 , mu , mu0 , nr , nrt
      REAL abst , rc
      COMPLEX ra , rb , rs , t
!     ..
!     .. External Subroutines ..
      EXTERNAL CLARGV , CLARTG , CLARTV , CLASET , CROT , CSCAL , XERBLA
!     ..
!     .. Intrinsic Functions ..
      INTRINSIC ABS , CONJG , MAX , MIN
!     ..
!     .. External Functions ..
      LOGICAL LSAME
      EXTERNAL LSAME
!     ..
!     .. Executable Statements ..
!
!     Test the input parameters
!
      wantb = LSAME(Vect,'B')
      wantq = LSAME(Vect,'Q') .OR. wantb
      wantpt = LSAME(Vect,'P') .OR. wantb
      wantc = Ncc>0
      klu1 = Kl + Ku + 1
      Info = 0
      IF ( .NOT.wantq .AND. .NOT.wantpt .AND. .NOT.LSAME(Vect,'N') )    &
     &     THEN
         Info = -1
      ELSEIF ( M<0 ) THEN
         Info = -2
      ELSEIF ( N<0 ) THEN
         Info = -3
      ELSEIF ( Ncc<0 ) THEN
         Info = -4
      ELSEIF ( Kl<0 ) THEN
         Info = -5
      ELSEIF ( Ku<0 ) THEN
         Info = -6
      ELSEIF ( Ldab<klu1 ) THEN
         Info = -8
      ELSEIF ( Ldq<1 .OR. wantq .AND. Ldq<MAX(1,M) ) THEN
         Info = -12
      ELSEIF ( Ldpt<1 .OR. wantpt .AND. Ldpt<MAX(1,N) ) THEN
         Info = -14
      ELSEIF ( Ldc<1 .OR. wantc .AND. Ldc<MAX(1,M) ) THEN
         Info = -16
      ENDIF
      IF ( Info/=0 ) THEN
         CALL XERBLA('CGBBRD',-Info)
         RETURN
      ENDIF
!
!     Initialize Q and P**H to the unit matrix, if needed
!
      IF ( wantq ) CALL CLASET('Full',M,M,CZERO,CONE,Q,Ldq)
      IF ( wantpt ) CALL CLASET('Full',N,N,CZERO,CONE,Pt,Ldpt)
!
!     Quick return if possible.
!
      IF ( M==0 .OR. N==0 ) RETURN
!
      minmn = MIN(M,N)
!
      IF ( Kl+Ku>1 ) THEN
!
!        Reduce to upper bidiagonal form if KU > 0; if KU = 0, reduce
!        first to lower bidiagonal form and then transform to upper
!        bidiagonal
!
         IF ( Ku>0 ) THEN
            ml0 = 1
            mu0 = 2
         ELSE
            ml0 = 2
            mu0 = 1
         ENDIF
!
!        Wherever possible, plane rotations are generated and applied in
!        vector operations of length NR over the index set J1:J2:KLU1.
!
!        The complex sines of the plane rotations are stored in WORK,
!        and the real cosines in RWORK.
!
         klm = MIN(M-1,Kl)
         kun = MIN(N-1,Ku)
         kb = klm + kun
         kb1 = kb + 1
         inca = kb1*Ldab
         nr = 0
         j1 = klm + 2
         j2 = 1 - kun
!
         DO i = 1 , minmn
!
!           Reduce i-th column and i-th row of matrix to bidiagonal form
!
            ml = klm + 1
            mu = kun + 1
            DO kk = 1 , kb
               j1 = j1 + kb
               j2 = j2 + kb
!
!              generate plane rotations to annihilate nonzero elements
!              which have been created below the band
!
               IF ( nr>0 ) CALL CLARGV(nr,Ab(klu1,j1-klm-1),inca,       &
     &                                 Work(j1),kb1,Rwork(j1),kb1)
!
!              apply plane rotations from the left
!
               DO l = 1 , kb
                  IF ( j2-klm+l-1>N ) THEN
                     nrt = nr - 1
                  ELSE
                     nrt = nr
                  ENDIF
                  IF ( nrt>0 ) CALL CLARTV(nrt,Ab(klu1-l,j1-klm+l-1),   &
     &                 inca,Ab(klu1-l+1,j1-klm+l-1),inca,Rwork(j1),     &
     &                 Work(j1),kb1)
               ENDDO
!
               IF ( ml>ml0 ) THEN
                  IF ( ml<=M-i+1 ) THEN
!
!                    generate plane rotation to annihilate a(i+ml-1,i)
!                    within the band, and apply rotation from the left
!
                     CALL CLARTG(Ab(Ku+ml-1,i),Ab(Ku+ml,i),Rwork(i+ml-1)&
     &                           ,Work(i+ml-1),ra)
                     Ab(Ku+ml-1,i) = ra
                     IF ( i<N )                                         &
     &                    CALL CROT(MIN(Ku+ml-2,N-i),Ab(Ku+ml-2,i+1),   &
     &                    Ldab-1,Ab(Ku+ml-1,i+1),Ldab-1,Rwork(i+ml-1),  &
     &                    Work(i+ml-1))
                  ENDIF
                  nr = nr + 1
                  j1 = j1 - kb1
               ENDIF
!
               IF ( wantq ) THEN
!
!                 accumulate product of plane rotations in Q
!
                  DO j = j1 , j2 , kb1
                     CALL CROT(M,Q(1,j-1),1,Q(1,j),1,Rwork(j),          &
     &                         CONJG(Work(j)))
                  ENDDO
               ENDIF
!
               IF ( wantc ) THEN
!
!                 apply plane rotations to C
!
                  DO j = j1 , j2 , kb1
                     CALL CROT(Ncc,C(j-1,1),Ldc,C(j,1),Ldc,Rwork(j),    &
     &                         Work(j))
                  ENDDO
               ENDIF
!
               IF ( j2+kun>N ) THEN
!
!                 adjust J2 to keep within the bounds of the matrix
!
                  nr = nr - 1
                  j2 = j2 - kb1
               ENDIF
!
               DO j = j1 , j2 , kb1
!
!                 create nonzero element a(j-1,j+ku) above the band
!                 and store it in WORK(n+1:2*n)
!
                  Work(j+kun) = Work(j)*Ab(1,j+kun)
                  Ab(1,j+kun) = Rwork(j)*Ab(1,j+kun)
               ENDDO
!
!              generate plane rotations to annihilate nonzero elements
!              which have been generated above the band
!
               IF ( nr>0 ) CALL CLARGV(nr,Ab(1,j1+kun-1),inca,          &
     &                                 Work(j1+kun),kb1,Rwork(j1+kun),  &
     &                                 kb1)
!
!              apply plane rotations from the right
!
               DO l = 1 , kb
                  IF ( j2+l-1>M ) THEN
                     nrt = nr - 1
                  ELSE
                     nrt = nr
                  ENDIF
                  IF ( nrt>0 ) CALL CLARTV(nrt,Ab(l+1,j1+kun-1),inca,   &
     &                 Ab(l,j1+kun),inca,Rwork(j1+kun),Work(j1+kun),kb1)
               ENDDO
!
               IF ( ml==ml0 .AND. mu>mu0 ) THEN
                  IF ( mu<=N-i+1 ) THEN
!
!                    generate plane rotation to annihilate a(i,i+mu-1)
!                    within the band, and apply rotation from the right
!
                     CALL CLARTG(Ab(Ku-mu+3,i+mu-2),Ab(Ku-mu+2,i+mu-1), &
     &                           Rwork(i+mu-1),Work(i+mu-1),ra)
                     Ab(Ku-mu+3,i+mu-2) = ra
                     CALL CROT(MIN(Kl+mu-2,M-i),Ab(Ku-mu+4,i+mu-2),1,   &
     &                         Ab(Ku-mu+3,i+mu-1),1,Rwork(i+mu-1),      &
     &                         Work(i+mu-1))
                  ENDIF
                  nr = nr + 1
                  j1 = j1 - kb1
               ENDIF
!
               IF ( wantpt ) THEN
!
!                 accumulate product of plane rotations in P**H
!
                  DO j = j1 , j2 , kb1
                     CALL CROT(N,Pt(j+kun-1,1),Ldpt,Pt(j+kun,1),Ldpt,   &
     &                         Rwork(j+kun),CONJG(Work(j+kun)))
                  ENDDO
               ENDIF
!
               IF ( j2+kb>M ) THEN
!
!                 adjust J2 to keep within the bounds of the matrix
!
                  nr = nr - 1
                  j2 = j2 - kb1
               ENDIF
!
               DO j = j1 , j2 , kb1
!
!                 create nonzero element a(j+kl+ku,j+ku-1) below the
!                 band and store it in WORK(1:n)
!
                  Work(j+kb) = Work(j+kun)*Ab(klu1,j+kun)
                  Ab(klu1,j+kun) = Rwork(j+kun)*Ab(klu1,j+kun)
               ENDDO
!
               IF ( ml>ml0 ) THEN
                  ml = ml - 1
               ELSE
                  mu = mu - 1
               ENDIF
            ENDDO
         ENDDO
      ENDIF
!
      IF ( Ku==0 .AND. Kl>0 ) THEN
!
!        A has been reduced to complex lower bidiagonal form
!
!        Transform lower bidiagonal form to upper bidiagonal by applying
!        plane rotations from the left, overwriting superdiagonal
!        elements on subdiagonal elements
!
         DO i = 1 , MIN(M-1,N)
            CALL CLARTG(Ab(1,i),Ab(2,i),rc,rs,ra)
            Ab(1,i) = ra
            IF ( i<N ) THEN
               Ab(2,i) = rs*Ab(1,i+1)
               Ab(1,i+1) = rc*Ab(1,i+1)
            ENDIF
            IF ( wantq ) CALL CROT(M,Q(1,i),1,Q(1,i+1),1,rc,CONJG(rs))
            IF ( wantc ) CALL CROT(Ncc,C(i,1),Ldc,C(i+1,1),Ldc,rc,rs)
         ENDDO
!
!        A has been reduced to complex upper bidiagonal form or is
!        diagonal
!
      ELSEIF ( Ku>0 .AND. M<N ) THEN
!
!           Annihilate a(m,m+1) by applying plane rotations from the
!           right
!
         rb = Ab(Ku,M+1)
         DO i = M , 1 , -1
            CALL CLARTG(Ab(Ku+1,i),rb,rc,rs,ra)
            Ab(Ku+1,i) = ra
            IF ( i>1 ) THEN
               rb = -CONJG(rs)*Ab(Ku,i)
               Ab(Ku,i) = rc*Ab(Ku,i)
            ENDIF
            IF ( wantpt ) CALL CROT(N,Pt(i,1),Ldpt,Pt(M+1,1),Ldpt,rc,   &
     &                              CONJG(rs))
         ENDDO
      ENDIF
!
!     Make diagonal and superdiagonal elements real, storing them in D
!     and E
!
      t = Ab(Ku+1,1)
      DO i = 1 , minmn
         abst = ABS(t)
         D(i) = abst
         IF ( abst/=ZERO ) THEN
            t = t/abst
         ELSE
            t = CONE
         ENDIF
         IF ( wantq ) CALL CSCAL(M,t,Q(1,i),1)
         IF ( wantc ) CALL CSCAL(Ncc,CONJG(t),C(i,1),Ldc)
         IF ( i<minmn ) THEN
            IF ( Ku==0 .AND. Kl==0 ) THEN
               E(i) = ZERO
               t = Ab(1,i+1)
            ELSE
               IF ( Ku==0 ) THEN
                  t = Ab(2,i)*CONJG(t)
               ELSE
                  t = Ab(Ku,i+1)*CONJG(t)
               ENDIF
               abst = ABS(t)
               E(i) = abst
               IF ( abst/=ZERO ) THEN
                  t = t/abst
               ELSE
                  t = CONE
               ENDIF
               IF ( wantpt ) CALL CSCAL(N,t,Pt(i+1,1),Ldpt)
               t = Ab(Ku+1,i+1)*CONJG(t)
            ENDIF
         ENDIF
      ENDDO
!
!     End of CGBBRD
!
      END SUBROUTINE CGBBRD


c       this file contains the following user-callable routines:
c
c
c       routine iddp_qrpiv computes the pivoted QR decomposition
c       of a matrix via Householder transformations,
c       stopping at a specified precision of the decomposition.
c
c       routine iddr_qrpiv computes the pivoted QR decomposition
c       of a matrix via Householder transformations,
c       stopping at a specified rank of the decomposition.
c
c       routine idd_qmatvec applies to a single vector
c       the Q matrix (or its transpose) in the QR decomposition
c       of a matrix, as described by the output of iddp_qrpiv
c       or iddr_qrpiv. If you're concerned about efficiency
c       and want to apply Q (or its transpose) to multiple vectors,
c       use idd_qmatmat instead.
c
c       routine idd_qmatmat applies
c       to multiple vectors collected together
c       as a matrix the Q matrix (or its transpose)
c       in the QR decomposition of a matrix, as described
c       by the output of iddp_qrpiv or iddr_qrpiv. If you don't want
c       to provide a work array and want to apply Q (or its transpose)
c       to a single vector, use idd_qmatvec instead.
c
c       routine idd_qinqr reconstructs the Q matrix
c       in a QR decomposition from the data generated
c       by iddp_qrpiv or iddr_qrpiv.
c
c       routine idd_permmult multiplies together a bunch
c       of permutations.
c
c
ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc
c
c
c
c

        subroutine idd_permmult(m,ind,n,indprod)
c
c       multiplies together the series of permutations in ind.
c
c       input:
c       m -- length of ind
c       ind(k) -- number of the slot with which to swap
c                 the k^th slot
c       n -- length of indprod and indprodinv
c
c       output:
c       indprod -- product of the permutations in ind,
c                  with the permutation swapping 1 and ind(1)
c                  taken leftmost in the product,
c                  that swapping 2 and ind(2) taken next leftmost,
c                  ..., that swapping krank and ind(krank)
c                  taken rightmost; indprod(k) is the number
c                  of the slot with which to swap the k^th slot
c                  in the product permutation
c
        implicit none
        integer m,n,ind(m),indprod(n),k,iswap
c
c
        do k = 1,n
          indprod(k) = k
        enddo ! k
c
        do k = m,1,-1
c
c         Swap indprod(k) and indprod(ind(k)).
c
          iswap = indprod(k)
          indprod(k) = indprod(ind(k))
          indprod(ind(k)) = iswap
c
        enddo ! k
c
c
        return
        end
c
c
c
c
        subroutine idd_qinqr(m,n,a,krank,q)
c
c       constructs the matrix q from iddp_qrpiv or iddr_qrpiv
c       (see the routine iddp_qrpiv or iddr_qrpiv
c       for more information).
c
c       input:
c       m -- first dimension of a; also, right now, q is m x m
c       n -- second dimension of a
c       a -- matrix output by iddp_qrpiv or iddr_qrpiv
c            (and denoted the same there)
c       krank -- numerical rank output by iddp_qrpiv or iddr_qrpiv
c                (and denoted the same there)
c
c       output:
c       q -- orthogonal matrix implicitly specified by the data in a
c            from iddp_qrpiv or iddr_qrpiv
c
c       Note:
c       Right now, this routine simply multiplies
c       one after another the krank Householder matrices
c       in the full QR decomposition of a,
c       in order to obtain the complete m x m Q factor in the QR.
c       This routine should instead use the following
c       (more elaborate but more efficient) scheme
c       to construct a q dimensioned q(krank,m); this scheme
c       was introduced by Robert Schreiber and Charles Van Loan
c       in "A Storage-Efficient _WY_ Representation
c       for Products of Householder Transformations,"
c       _SIAM Journal on Scientific and Statistical Computing_,
c       Vol. 10, No. 1, pp. 53-57, January, 1989:
c
c       Theorem 1. Suppose that Q = _1_ + YTY^T is
c       an m x m orthogonal real matrix,
c       where Y is an m x k real matrix
c       and T is a k x k upper triangular real matrix.
c       Suppose also that P = _1_ - 2 v v^T is
c       a real Householder matrix and Q_+ = QP,
c       where v is an m x 1 real vector,
c       normalized so that v^T v = 1.
c       Then, Q_+ = _1_ + Y_+ T_+ Y_+^T,
c       where Y_+ = (Y v) is the m x (k+1) matrix
c       formed by adjoining v to the right of Y,
c                 ( T   z )
c       and T_+ = (       ) is
c                 ( 0  -2 )
c       the (k+1) x (k+1) upper triangular matrix
c       formed by adjoining z to the right of T
c       and the vector (0 ... 0 -2) with k zeroes below (T z),
c       where z = -2 T Y^T v.
c
c       Now, suppose that A is a (rank-deficient) matrix
c       whose complete QR decomposition has
c       the blockwise partioned form
c           ( Q_11 Q_12 ) ( R_11 R_12 )   ( Q_11 )
c       A = (           ) (           ) = (      ) (R_11 R_12).
c           ( Q_21 Q_22 ) (  0    0   )   ( Q_21 )
c       Then, the only blocks of the orthogonal factor
c       in the above QR decomposition of A that matter are
c                                                        ( Q_11 )
c       Q_11 and Q_21, _i.e._, only the block of columns (      )
c                                                        ( Q_21 )
c       interests us.
c       Suppose in addition that Q_11 is a k x k matrix,
c       Q_21 is an (m-k) x k matrix, and that
c       ( Q_11 Q_12 )
c       (           ) = _1_ + YTY^T, as in Theorem 1 above.
c       ( Q_21 Q_22 )
c       Then, Q_11 = _1_ + Y_1 T Y_1^T
c       and Q_21 = Y_2 T Y_1^T,
c       where Y_1 is the k x k matrix and Y_2 is the (m-k) x k matrix
c                   ( Y_1 )
c       so that Y = (     ).
c                   ( Y_2 )
c
c       So, you can calculate T and Y via the above recursions,
c       and then use these to compute the desired Q_11 and Q_21.
c
c
        implicit none
        integer m,n,krank,j,k,mm,ifrescal
        real*8 a(m,n),q(m,m),scal
c
c
c       Zero all of the entries of q.
c
        do k = 1,m
          do j = 1,m
            q(j,k) = 0
          enddo ! j
        enddo ! k
c
c
c       Place 1's along the diagonal of q.
c
        do k = 1,m
          q(k,k) = 1
        enddo ! k
c
c
c       Apply the krank Householder transformations stored in a.
c
        do k = krank,1,-1
          do j = k,m
            mm = m-k+1
            ifrescal = 1
            if(k .lt. m)
     1       call idd_houseapp(mm,a(k+1,k),q(k,j),ifrescal,scal,q(k,j))
          enddo ! j
        enddo ! k
c
c
        return
        end
c
c
c
c
        subroutine idd_qmatvec(iftranspose,m,n,a,krank,v)
c
c       applies to a single vector the Q matrix (or its transpose)
c       which the routine iddp_qrpiv or iddr_qrpiv has stored
c       in a triangle of the matrix it produces (stored, incidentally,
c       as data for applying a bunch of Householder reflections).
c       Use the routine qmatmat to apply the Q matrix
c       (or its transpose)
c       to a bunch of vectors collected together as a matrix,
c       if you're concerned about efficiency.
c
c       input:
c       iftranspose -- set to 0 for applying Q;
c                      set to 1 for applying the transpose of Q
c       m -- first dimension of a and length of v
c       n -- second dimension of a
c       a -- data describing the qr decomposition of a matrix,
c            as produced by iddp_qrpiv or iddr_qrpiv
c       krank -- numerical rank
c       v -- vector to which Q (or its transpose) is to be applied
c
c       output:
c       v -- vector to which Q (or its transpose) has been applied
c
        implicit none
        save
        integer m,n,krank,k,ifrescal,mm,iftranspose
        real*8 a(m,n),v(m),scal
c
c
        ifrescal = 1
c
c
        if(iftranspose .eq. 0) then
c
          do k = krank,1,-1
            mm = m-k+1
            if(k .lt. m)
     1       call idd_houseapp(mm,a(k+1,k),v(k),ifrescal,scal,v(k))
          enddo ! k
c
        endif
c
c
        if(iftranspose .eq. 1) then
c
          do k = 1,krank
            mm = m-k+1
            if(k .lt. m)
     1       call idd_houseapp(mm,a(k+1,k),v(k),ifrescal,scal,v(k))
          enddo ! k
c
        endif
c
c
        return
        end
c
c
c
c
        subroutine idd_qmatmat(iftranspose,m,n,a,krank,l,b,work)
c
c       applies to a bunch of vectors collected together as a matrix
c       the Q matrix (or its transpose) which the routine iddp_qrpiv or
c       iddr_qrpiv has stored in a triangle of the matrix it produces
c       (stored, incidentally, as data for applying a bunch
c       of Householder reflections).
c       Use the routine qmatvec to apply the Q matrix
c       (or its transpose)
c       to a single vector, if you'd rather not provide a work array.
c
c       input:
c       iftranspose -- set to 0 for applying Q;
c                      set to 1 for applying the transpose of Q
c       m -- first dimension of both a and b
c       n -- second dimension of a
c       a -- data describing the qr decomposition of a matrix,
c            as produced by iddp_qrpiv or iddr_qrpiv
c       krank -- numerical rank
c       l -- second dimension of b
c       b -- matrix to which Q (or its transpose) is to be applied
c
c       output:
c       b -- matrix to which Q (or its transpose) has been applied
c
c       work:
c       work -- must be at least krank real*8 elements long
c
        implicit none
        save
        integer l,m,n,krank,j,k,ifrescal,mm,iftranspose
        real*8 a(m,n),b(m,l),work(krank)
c
c
        if(iftranspose .eq. 0) then
c
c
c         Handle the first iteration, j = 1,
c         calculating all scals (ifrescal = 1).
c
          ifrescal = 1
c
          j = 1
c
          do k = krank,1,-1
            if(k .lt. m) then
              mm = m-k+1
              call idd_houseapp(mm,a(k+1,k),b(k,j),ifrescal,
     1                          work(k),b(k,j))
            endif
          enddo ! k
c
c
          if(l .gt. 1) then
c
c           Handle the other iterations, j > 1,
c           using the scals just computed (ifrescal = 0).
c
            ifrescal = 0
c
            do j = 2,l
c
              do k = krank,1,-1
                if(k .lt. m) then
                  mm = m-k+1
                  call idd_houseapp(mm,a(k+1,k),b(k,j),ifrescal,
     1                              work(k),b(k,j))
                endif
              enddo ! k
c
            enddo ! j
c
          endif ! j .gt. 1
c
c
        endif ! iftranspose .eq. 0
c
c
        if(iftranspose .eq. 1) then
c
c
c         Handle the first iteration, j = 1,
c         calculating all scals (ifrescal = 1).
c
          ifrescal = 1
c
          j = 1
c
          do k = 1,krank
            if(k .lt. m) then
              mm = m-k+1
              call idd_houseapp(mm,a(k+1,k),b(k,j),ifrescal,
     1                          work(k),b(k,j))
            endif
          enddo ! k
c
c
          if(l .gt. 1) then
c
c           Handle the other iterations, j > 1,
c           using the scals just computed (ifrescal = 0).
c
            ifrescal = 0
c
            do j = 2,l
c
              do k = 1,krank
                if(k .lt. m) then
                  mm = m-k+1
                  call idd_houseapp(mm,a(k+1,k),b(k,j),ifrescal,
     1                              work(k),b(k,j))
                endif
              enddo ! k
c
            enddo ! j
c
          endif ! j .gt. 1
c
c
        endif ! iftranspose .eq. 1
c
c
        return
        end
c
c
c
c
        subroutine iddp_qrpiv(eps,m,n,a,krank,ind,ss)
c
c       computes the pivoted QR decomposition
c       of the matrix input into a, using Householder transformations,
c       _i.e._, transforms the matrix a from its input value in
c       to the matrix out with entry
c
c                               m
c       out(j,indprod(k))  =  Sigma  q(l,j) * in(l,k),
c                              l=1
c
c       for all j = 1, ..., krank, and k = 1, ..., n,
c
c       where in = the a from before the routine runs,
c       out = the a from after the routine runs,
c       out(j,k) = 0 when j > k (so that out is triangular),
c       q(1:m,1), ..., q(1:m,krank) are orthonormal,
c       indprod is the product of the permutations given by ind,
c       (as computable via the routine permmult,
c       with the permutation swapping 1 and ind(1) taken leftmost
c       in the product, that swapping 2 and ind(2) taken next leftmost,
c       ..., that swapping krank and ind(krank) taken rightmost),
c       and with the matrix out satisfying
c
c                   krank
c       in(j,k)  =  Sigma  q(j,l) * out(l,indprod(k))  +  epsilon(j,k),
c                    l=1
c
c       for all j = 1, ..., m, and k = 1, ..., n,
c
c       for some matrix epsilon such that
c       the root-sum-square of the entries of epsilon
c       <= the root-sum-square of the entries of in * eps.
c       Well, technically, this routine outputs the Householder vectors
c       (or, rather, their second through last entries)
c       in the part of a that is supposed to get zeroed, that is,
c       in a(j,k) with m >= j > k >= 1.
c
c       input:
c       eps -- relative precision of the resulting QR decomposition
c       m -- first dimension of a and q
c       n -- second dimension of a
c       a -- matrix whose QR decomposition gets computed
c
c       output:
c       a -- triangular (R) factor in the QR decompositon
c            of the matrix input into the same storage locations,
c            with the Householder vectors stored in the part of a
c            that would otherwise consist entirely of zeroes, that is,
c            in a(j,k) with m >= j > k >= 1
c       krank -- numerical rank
c       ind(k) -- index of the k^th pivot vector;
c                 the following code segment will correctly rearrange
c                 the product b of q and the upper triangle of out
c                 so that b matches the input matrix in
c                 to relative precision eps:
c
c                 copy the non-rearranged product of q and out into b
c                 set k to krank
c                 [start of loop]
c                   swap b(1:m,k) and b(1:m,ind(k))
c                   decrement k by 1
c                 if k > 0, then go to [start of loop]
c
c       work:
c       ss -- must be at least n real*8 words long
c
c       _N.B._: This routine outputs the Householder vectors
c       (or, rather, their second through last entries)
c       in the part of a that is supposed to get zeroed, that is,
c       in a(j,k) with m >= j > k >= 1.
c
c       reference:
c       Golub and Van Loan, "Matrix Computations," 3rd edition,
c            Johns Hopkins University Press, 1996, Chapter 5.
c
        implicit none
        integer n,m,ind(n),krank,k,j,kpiv,mm,nupdate,ifrescal
        real*8 a(m,n),ss(n),eps,feps,ssmax,scal,ssmaxin,rswap
c
c
        feps = .1d-16
c
c
c       Compute the sum of squares of the entries in each column of a,
c       the maximum of all such sums, and find the first pivot
c       (column with the greatest such sum).
c
        ssmax = 0
        kpiv = 1
c
        do k = 1,n
c
          ss(k) = 0
          do j = 1,m
            ss(k) = ss(k)+a(j,k)**2
          enddo ! j
c
          if(ss(k) .gt. ssmax) then
            ssmax = ss(k)
            kpiv = k
          endif
c
        enddo ! k
c
        ssmaxin = ssmax
c
        nupdate = 0
c
c
c       While ssmax > eps**2*ssmaxin, krank < m, and krank < n,
c       do the following block of code,
c       which ends at the statement labeled 2000.
c
        krank = 0
 1000   continue
c
        if(ssmax .le. eps**2*ssmaxin
     1   .or. krank .ge. m .or. krank .ge. n) goto 2000
        krank = krank+1
c
c
          mm = m-krank+1
c
c
c         Perform the pivoting.
c
          ind(krank) = kpiv
c
c         Swap a(1:m,krank) and a(1:m,kpiv).
c
          do j = 1,m
            rswap = a(j,krank)
            a(j,krank) = a(j,kpiv)
            a(j,kpiv) = rswap
          enddo ! j
c
c         Swap ss(krank) and ss(kpiv).
c
          rswap = ss(krank)
          ss(krank) = ss(kpiv)
          ss(kpiv) = rswap
c
c
          if(krank .lt. m) then
c
c
c           Compute the data for the Householder transformation
c           which will zero a(krank+1,krank), ..., a(m,krank)
c           when applied to a, replacing a(krank,krank)
c           with the first entry of the result of the application
c           of the Householder matrix to a(krank:m,krank),
c           and storing entries 2 to mm of the Householder vector
c           in a(krank+1,krank), ..., a(m,krank)
c           (which otherwise would get zeroed upon application
c           of the Householder transformation).
c
            call idd_house(mm,a(krank,krank),a(krank,krank),
     1                     a(krank+1,krank),scal)
            ifrescal = 0
c
c
c           Apply the Householder transformation
c           to the lower right submatrix of a
c           with upper leftmost entry at position (krank,krank+1).
c
            if(krank .lt. n) then
              do k = krank+1,n
                call idd_houseapp(mm,a(krank+1,krank),a(krank,k),
     1                            ifrescal,scal,a(krank,k))
              enddo ! k
            endif
c
c
c           Update the sums-of-squares array ss.
c
            do k = krank,n
              ss(k) = ss(k)-a(krank,k)**2
            enddo ! k
c
c
c           Find the pivot (column with the greatest sum of squares
c           of its entries).
c
            ssmax = 0
            kpiv = krank+1
c
            if(krank .lt. n) then
c
              do k = krank+1,n
c
                if(ss(k) .gt. ssmax) then
                  ssmax = ss(k)
                  kpiv = k
                endif
c
              enddo ! k
c
            endif ! krank .lt. n
c
c
c           Recompute the sums-of-squares and the pivot
c           when ssmax first falls below
c           sqrt((1000*feps)^2) * ssmaxin
c           and when ssmax first falls below
c           ((1000*feps)^2) * ssmaxin.
c
            if(
     1       (ssmax .lt. sqrt((1000*feps)**2) * ssmaxin
     2        .and. nupdate .eq. 0) .or.
     3       (ssmax .lt. ((1000*feps)**2) * ssmaxin
     4        .and. nupdate .eq. 1)
     5      ) then
c
              nupdate = nupdate+1
c
              ssmax = 0
              kpiv = krank+1
c
              if(krank .lt. n) then
c
                do k = krank+1,n
c
                  ss(k) = 0
                  do j = krank+1,m
                    ss(k) = ss(k)+a(j,k)**2
                  enddo ! j
c
                  if(ss(k) .gt. ssmax) then
                    ssmax = ss(k)
                    kpiv = k
                  endif
c
                enddo ! k
c
              endif ! krank .lt. n
c
            endif
c
c
          endif ! krank .lt. m
c
c
        goto 1000
 2000   continue
c
c
        return
        end
c
c
c
c
        subroutine iddr_qrpiv(m,n,a,krank,ind,ss)
c
c       computes the pivoted QR decomposition
c       of the matrix input into a, using Householder transformations,
c       _i.e._, transforms the matrix a from its input value in
c       to the matrix out with entry
c
c                               m
c       out(j,indprod(k))  =  Sigma  q(l,j) * in(l,k),
c                              l=1
c
c       for all j = 1, ..., krank, and k = 1, ..., n,
c
c       where in = the a from before the routine runs,
c       out = the a from after the routine runs,
c       out(j,k) = 0 when j > k (so that out is triangular),
c       q(1:m,1), ..., q(1:m,krank) are orthonormal,
c       indprod is the product of the permutations given by ind,
c       (as computable via the routine permmult,
c       with the permutation swapping 1 and ind(1) taken leftmost
c       in the product, that swapping 2 and ind(2) taken next leftmost,
c       ..., that swapping krank and ind(krank) taken rightmost),
c       and with the matrix out satisfying
c
c                  min(krank,m,n)
c       in(j,k)  =     Sigma      q(j,l) * out(l,indprod(k))
c                       l=1
c
c                +  epsilon(j,k),
c
c       for all j = 1, ..., m, and k = 1, ..., n,
c
c       for some matrix epsilon whose norm is (hopefully) minimized
c       by the pivoting procedure.
c       Well, technically, this routine outputs the Householder vectors
c       (or, rather, their second through last entries)
c       in the part of a that is supposed to get zeroed, that is,
c       in a(j,k) with m >= j > k >= 1.
c
c       input:
c       m -- first dimension of a and q
c       n -- second dimension of a
c       a -- matrix whose QR decomposition gets computed
c       krank -- desired rank of the output matrix
c                (please note that if krank > m or krank > n,
c                then the rank of the output matrix will be
c                less than krank)
c
c       output:
c       a -- triangular (R) factor in the QR decompositon
c            of the matrix input into the same storage locations,
c            with the Householder vectors stored in the part of a
c            that would otherwise consist entirely of zeroes, that is,
c            in a(j,k) with m >= j > k >= 1
c       ind(k) -- index of the k^th pivot vector;
c                 the following code segment will correctly rearrange
c                 the product b of q and the upper triangle of out
c                 so that b best matches the input matrix in:
c
c                 copy the non-rearranged product of q and out into b
c                 set k to krank
c                 [start of loop]
c                   swap b(1:m,k) and b(1:m,ind(k))
c                   decrement k by 1
c                 if k > 0, then go to [start of loop]
c
c       work:
c       ss -- must be at least n real*8 words long
c
c       _N.B._: This routine outputs the Householder vectors
c       (or, rather, their second through last entries)
c       in the part of a that is supposed to get zeroed, that is,
c       in a(j,k) with m >= j > k >= 1.
c
c       reference:
c       Golub and Van Loan, "Matrix Computations," 3rd edition,
c            Johns Hopkins University Press, 1996, Chapter 5.
c
        implicit none
        integer n,m,ind(n),krank,k,j,kpiv,mm,nupdate,ifrescal,
     1          loops,loop
        real*8 a(m,n),ss(n),ssmax,scal,ssmaxin,rswap,feps
c
c
        feps = .1d-16
c
c
c       Compute the sum of squares of the entries in each column of a,
c       the maximum of all such sums, and find the first pivot
c       (column with the greatest such sum).
c
        ssmax = 0
        kpiv = 1
c
        do k = 1,n
c
          ss(k) = 0
          do j = 1,m
            ss(k) = ss(k)+a(j,k)**2
          enddo ! j
c
          if(ss(k) .gt. ssmax) then
            ssmax = ss(k)
            kpiv = k
          endif
c
        enddo ! k
c
        ssmaxin = ssmax
c
        nupdate = 0
c
c
c       Set loops = min(krank,m,n).
c
        loops = krank
        if(m .lt. loops) loops = m
        if(n .lt. loops) loops = n
c
        do loop = 1,loops
c
c
          mm = m-loop+1
c
c
c         Perform the pivoting.
c
          ind(loop) = kpiv
c
c         Swap a(1:m,loop) and a(1:m,kpiv).
c
          do j = 1,m
            rswap = a(j,loop)
            a(j,loop) = a(j,kpiv)
            a(j,kpiv) = rswap
          enddo ! j
c
c         Swap ss(loop) and ss(kpiv).
c
          rswap = ss(loop)
          ss(loop) = ss(kpiv)
          ss(kpiv) = rswap
c
c
          if(loop .lt. m) then
c
c
c           Compute the data for the Householder transformation
c           which will zero a(loop+1,loop), ..., a(m,loop)
c           when applied to a, replacing a(loop,loop)
c           with the first entry of the result of the application
c           of the Householder matrix to a(loop:m,loop),
c           and storing entries 2 to mm of the Householder vector
c           in a(loop+1,loop), ..., a(m,loop)
c           (which otherwise would get zeroed upon application
c           of the Householder transformation).
c
            call idd_house(mm,a(loop,loop),a(loop,loop),
     1                     a(loop+1,loop),scal)
            ifrescal = 0
c
c
c           Apply the Householder transformation
c           to the lower right submatrix of a
c           with upper leftmost entry at position (loop,loop+1).
c
            if(loop .lt. n) then
              do k = loop+1,n
                call idd_houseapp(mm,a(loop+1,loop),a(loop,k),
     1                            ifrescal,scal,a(loop,k))
              enddo ! k
            endif
c
c
c           Update the sums-of-squares array ss.
c
            do k = loop,n
              ss(k) = ss(k)-a(loop,k)**2
            enddo ! k
c
c
c           Find the pivot (column with the greatest sum of squares
c           of its entries).
c
            ssmax = 0
            kpiv = loop+1
c
            if(loop .lt. n) then
c
              do k = loop+1,n
c
                if(ss(k) .gt. ssmax) then
                  ssmax = ss(k)
                  kpiv = k
                endif
c
              enddo ! k
c
            endif ! loop .lt. n
c
c
c           Recompute the sums-of-squares and the pivot
c           when ssmax first falls below
c           sqrt((1000*feps)^2) * ssmaxin
c           and when ssmax first falls below
c           ((1000*feps)^2) * ssmaxin.
c
            if(
     1       (ssmax .lt. sqrt((1000*feps)**2) * ssmaxin
     2        .and. nupdate .eq. 0) .or.
     3       (ssmax .lt. ((1000*feps)**2) * ssmaxin
     4        .and. nupdate .eq. 1)
     5      ) then
c
              nupdate = nupdate+1
c
              ssmax = 0
              kpiv = loop+1
c
              if(loop .lt. n) then
c
                do k = loop+1,n
c
                  ss(k) = 0
                  do j = loop+1,m
                    ss(k) = ss(k)+a(j,k)**2
                  enddo ! j
c
                  if(ss(k) .gt. ssmax) then
                    ssmax = ss(k)
                    kpiv = k
                  endif
c
                enddo ! k
c
              endif ! loop .lt. n
c
            endif
c
c
          endif ! loop .lt. m
c
c
        enddo ! loop
c
c
        return
        end


      SUBROUTINE SROTG(SA,SB,C,S)
*     .. Scalar Arguments ..
      REAL C,S,SA,SB
*     ..
*
*  Purpose
*  =======
*
*     construct givens plane rotation.
*     jack dongarra, linpack, 3/11/78.
*
*
*     .. Local Scalars ..
      REAL R,ROE,SCALE,Z
*     ..
*     .. Intrinsic Functions ..
      INTRINSIC ABS,SIGN,SQRT
*     ..
      ROE = SB
      IF (ABS(SA).GT.ABS(SB)) ROE = SA
      SCALE = ABS(SA) + ABS(SB)
      IF (SCALE.NE.0.0) GO TO 10
      C = 1.0
      S = 0.0
      R = 0.0
      Z = 0.0
      GO TO 20
   10 R = SCALE*SQRT((SA/SCALE)**2+ (SB/SCALE)**2)
      R = SIGN(1.0,ROE)*R
      C = SA/R
      S = SB/R
      Z = 1.0
      IF (ABS(SA).GT.ABS(SB)) Z = S
      IF (ABS(SB).GE.ABS(SA) .AND. C.NE.0.0) Z = 1.0/C
   20 SA = R
      SB = Z
      RETURN
      END


    SUBROUTINE ORTHMGS(Q,NQ,R,NR,NC,NN,B,NWM,MAXA,X,TT)
!
!-----[--.----+----.----+----.-----------------------------------------]
!      PURPOSE: USE GRAM-SCHMIDT ORTHOGONALIzATION Q

!      INPUTS:
!       Q(NN,NQ)    - ORIGIN ORTHOGONALIzATION MATRIx
!        NQ            - NUMBER OF COLUMNS TO ORTHOGONALIzE IN Q
!       R(NN,NR)    - ORIGIN ORTHOGONALIzATION MATRIx
!       NR          - NUMBER OF COLUMNS OF MATRIX R
!        NC            - NUMBER OF COLUMNS TO ORTHOGONALIzE IN Q
!                   - NC.GT.0, IF HAVE THE EIGENVALUE
!       NN            - NUMBER OF ROwS IN Q, R
!       B(*)        - MASS MATRIx
!        NWM         - NUMBER FOR MASS STORE

!      SCRATCH:
!        TT(NN)  - WORKING vECTOR

!      OUTPUTS:
!         X  - ORTHOGONALIzATION MATRIx
!-----[--.----+----.----+----.-----------------------------------------]
!
    IMPLICIT NONE
    INTEGER, INTENT(IN):: NN,NQ,NR,NC,NWM,MAXA(*)
    REAL(8), INTENT(IN):: Q(NN,*),R(NN,*),B(*)
    REAL*8   ALPHA,TT(NN),X(NN),Y(NN)
    INTEGER I,J,K

    Y = X

    DO K=1,NQ
      CALL MULT(TT,B(1),Q(1,K),MAXA,NN,NWM)
      ALPHA=DOT_PRODUCT(X,TT)
      DO I=1,NN
        Y(I)=Y(I)-ALPHA*Q(I,K)
      END DO
    END DO
    DO K=1,NC
      CALL MULT(TT,B(1),R(1,K),MAXA,NN,NWM)
      ALPHA=DOT_PRODUCT(X,TT)
      DO I=1,NN
        Y(I)=Y(I)-ALPHA*R(I,K)
      END DO
    END DO

    X = Y

    END