SUBROUTINE STEPQS(N,NFE,IFLAG,LENQR,START,CRASH,HOLD,H,
     $            WK,RELERR,ABSERR,S,Y,YP,YOLD,YPOLD,A,QR,PIVOT,PP,
     $            RHOVEC,Z0,DZ,T,WORK,SSPAR,PAR,IPAR)
C
C SUBROUTINE  STEPQS  TAKES ONE STEP ALONG THE ZERO CURVE OF THE
C HOMOTOPY MAP  RHO(X,LAMBDA)  USING A PREDICTOR-CORRECTOR ALGORITHM.
C THE PREDICTOR USES A HERMITE CUBIC INTERPOLANT, AND THE CORRECTOR
C RETURNS TO THE ZERO CURVE USING A NEWTON ITERATION, REMAINING
C IN A HYPERPLANE PERPENDICULAR TO THE MOST RECENT TANGENT VECTOR.
C  STEPQS  ALSO ESTIMATES A STEP SIZE  H  FOR THE NEXT STEP ALONG THE
C ZERO CURVE.
C
C
C ON INPUT:
C
C N = DIMENSION OF  X.
C
C NFE = NUMBER OF JACOBIAN MATRIX EVALUATIONS.
C
C IFLAG = -2, -1, OR 0, INDICATING THE PROBLEM TYPE.
C
C LENQR = THE LENGTH OF THE ONE DIMENSIONAL ARRAY  QR.
C
C START = .TRUE. ON FIRST CALL TO  STEPQS, .FALSE. OTHERWISE.
C         SHOULD NOT BE MODIFIED BY THE USER AFTER THE FIRST CALL.
C
C HOLD = ||Y - YOLD|| ; SHOULD NOT BE MODIFIED BY THE USER.
C
C H = UPPER LIMIT ON LENGTH OF STEP THAT WILL BE ATTEMPTED.  H  MUST
C    BE SET TO A POSITIVE NUMBER ON THE FIRST CALL TO  STEPQS.
C    THEREAFTER,  STEPQS  CALCULATES AN OPTIMAL VALUE FOR  H, AND  H
C    SHOULD NOT BE MODIFIED BY THE USER.
C
C WK = APPROXIMATE CURVATURE FOR THE LAST STEP (COMPUTED BY PREVIOUS
C    CALL TO  STEPQS).  UNDEFINED ON FIRST CALL.  SHOULD NOT BE
C    MODIFIED BY THE USER.
C
C RELERR, ABSERR = RELATIVE AND ABSOLUTE ERROR VALUES.  THE ITERATION
C    IS CONSIDERED TO HAVE CONVERGED WHEN A POINT  Z=(X,LAMBDA)  IS
C    FOUND SUCH THAT
C       ||DZ|| .LE. RELERR*||Z|| + ABSERR,
C    WHERE  DZ  IS THE LAST NEWTON STEP.
C
C S  = (APPROXIMATE) ARC LENGTH ALONG THE HOMOTOPY ZERO CURVE UP TO
C    Y(S) = (X(S),LAMBDA(S)).
C
C Y(1:N+1) = PREVIOUS POINT (X(S),LAMBDA(S)) FOUND ON THE ZERO CURVE
C    OF THE HOMOTOPY MAP.
C
C YP(1:N+1) = UNIT TANGENT VECTOR TO THE ZERO CURVE OF THE HOMOTOPY
C    MAP AT  Y.  INPUT IN THIS VECTOR IS NOT USED ON THE FIRST CALL
C    TO  STEPQS.
C
C YOLD(1:N+1) = A POINT BEFORE  Y  ON THE ZERO CURVE OF THE HOMOTOPY
C    MAP.  INPUT IN THIS VECTOR IS NOT USED ON THE FIRST CALL TO
C    STEPQS.
C
C YPOLD(1:N+1) = UNIT TANGENT VECTOR TO THE ZERO CURVE OF THE
C    HOMOTOPY MAP AT  YOLD.
C
C A(1:N) = PARAMETER VECTOR IN THE HOMOTOPY MAP.
C
C QR(1:LENQR)  IS A WORK ARRAY CONTAINING THE N X N SYMMETRIC
C    JACOBIAN MATRIX WITH RESPECT TO X STORED IN PACKED SKYLINE
C    STORAGE FORMAT.  LENQR  AND  PIVOT  DESCRIBE THE DATA
C    STRUCTURE IN  QR.  (SEE SUBROUTINE  PCGQS  FOR A DESCRIPTION
C    OF THIS DATA STRUCTURE).
C
C PIVOT(1:N+2)  IS A WORK ARRAY WHOSE FIRST N+1 COMPONENTS CONTAIN
C    THE INDICES OF THE DIAGONAL ELEMENTS OF THE N X N SYMMETRIC
C    JACOBIAN MATRIX (WITH RESPECT TO X) WITHIN  QR.
C
C PP(1:N)  IS A WORK ARRAY CONTAINING THE NEGATIVE OF THE LAST COLUMN
C    OF THE JACOBIAN MATRIX  -[D RHO/D LAMBDA].
C
C RHOVEC(1:N+1), Z0(1:N+1), DZ(1:N+1), T(1:N+1)  ARE ALL WORK ARRAYS
C    USED BY  STEPQS, TANGQS, AND ROOTQS  TO CALCULATE THE TANGENT
C    VECTORS AND NEWTON STEPS.
C
C WORK(1:8*(N+1)+LENQR)  IS A WORK ARRAY USED BY THE CONJUGATE GRADIENT
C    ALGORITHM TO SOLVE LINEAR SYSTEMS.
C
C SSPAR(1:4) = PARAMETERS USED FOR COMPUTATION OF THE OPTIMAL STEP SIZE.
C    SSPAR(1) = HMIN, SSPAR(2) = HMAX, SSPAR(3) = BMIN, SSPAR(4) = BMAX.
C    THE OPTIMAL STEP  H  IS RESTRICTED SUCH THAT
C       HMIN .LE. H .LE. HMAX, AND  BMIN*HOLD .LE. H .LE. BMAX*HOLD.
C
C PAR(1:*)  AND  IPAR(1:*)  ARE ARRAYS FOR (OPTIONAL) USER PARAMETERS,
C    WHICH ARE SIMPLY PASSED THROUGH TO THE USER WRITTEN SUBROUTINES
C    RHO, RHOJS.
C
C
C ON OUTPUT:
C
C N, LENQR, A  ARE UNCHANGED.
C
C NFE HAS BEEN UPDATED.
C
C IFLAG
C
C    = -2, -1, OR 0 (UNCHANGED) ON A NORMAL RETURN.
C
C    = 4 IF A JACOBIAN MATRIX WITH RANK <  N  HAS OCCURRED.  THE
C        ITERATION WAS NOT COMPLETED.
C
C    = 6 IF THE ITERATION FAILED TO CONVERGE.
C
C START = .FALSE. ON A NORMAL RETURN.
C
C CRASH
C
C    = .FALSE. ON A NORMAL RETURN.
C
C    = .TRUE. IF THE STEP SIZE  H  WAS TOO SMALL.  H  HAS BEEN
C      INCREASED TO AN ACCEPTABLE VALUE, WITH WHICH  STEPQS  MAY BE
C      CALLED AGAIN.
C
C    = .TRUE. IF  RELERR  AND/OR  ABSERR  WERE TOO SMALL.  THEY HAVE
C      BEEN INCREASED TO ACCEPTABLE VALUES, WITH WHICH  STEPQS  MAY
C      BE CALLED AGAIN.
C
C HOLD = ||Y-YOLD||.
C
C H = OPTIMAL VALUE FOR NEXT STEP TO BE ATTEMPTED.  NORMALLY  H  SHOULD
C     NOT BE MODIFIED BY THE USER.
C
C WK = APPROXIMATE CURVATURE FOR THE STEP TAKEN BY  STEPQS.
C
C S = (APPROXIMATE) ARC LENGTH ALONG THE ZERO CURVE OF THE HOMOTOPY
C     MAP UP TO THE LATEST POINT FOUND, WHICH IS RETURNED IN  Y.
C
C RELERR, ABSERR  ARE UNCHANGED ON A NORMAL RETURN.  THEY ARE POSSIBLY
C     CHANGED IF  CRASH  = .TRUE. (SEE DESCRIPTION OF  CRASH  ABOVE).
C
C Y, YP, YOLD, YPOLD  CONTAIN THE TWO MOST RECENT POINTS AND TANGENT
C     VECTORS FOUND ON THE ZERO CURVE OF THE HOMOTOPY MAP.
C
C
C CALLS  D1MACH, DAXPY, DCOPY, DDOT, DNRM2, DSCAL, F (OR RHO), FJACS
C     (OR RHOJS), PCGQS, TANGQS.
C
C ***** DECLARATIONS *****
C
C     FUNCTION DECLARATIONS
C
        DOUBLE PRECISION D1MACH, DDOT, DNRM2, QOFS
C
C     LOCAL VARIABLES
C
        DOUBLE PRECISION ALPHA, CORDIS, DD001, DD0011,DD01,DD011,DELS,
     $    FOURU, GAMMA, HFAIL, HTEMP, IDLERR, LAMBDA, OMEGA, ONE, P0,
     $    P1, PP0, PP1, SIGMA, TEMP, THETA, TWOU, WKOLD, XSTEP
        INTEGER I, ITCNT, LITFH, J, LK, LST, NP1, PCGWK, ZU
        LOGICAL FAILED
C
C     SCALAR ARGUMENTS
C
        INTEGER N, NFE, IFLAG, LENQR
        LOGICAL START, CRASH
        DOUBLE PRECISION HOLD, H, WK, RELERR, ABSERR, S
C
C     ARRAY DECLARATIONS
C
        DOUBLE PRECISION Y(N+1), YP(N+1), YOLD(N+1), YPOLD(N+1),
     $    A(N), QR(LENQR), PP(N), RHOVEC(N+1), Z0(N+1), DZ(N+1),
     $    T(N+1), WORK(8*(N+1)+LENQR), SSPAR(4), PAR(1)
        INTEGER PIVOT(N+2), IPAR(1)
        REAL WRGE(8),ACOF(12)
C
        SAVE
C
        DATA WRGE  /
     $   .8735115E+00, .1531947E+00, .3191815E-01, .3339946E-10,
     $   .4677788E+00, .6970123E-03, .1980863E-05, .1122789E-08/
        DATA ACOF  /
     $   .9043128E+00,-.7075675E+00,-.4667383E+01,-.3677482E+01,
     $   .8516099E+00,-.1953119E+00,-.4830636E+01,-.9770528E+00,
     $   .1040061E+01, .3793395E-01, .1042177E+01, .4450706E-01/
C
C ***** END OF DECLARATIONS *****
C
C DEFINITION OF HERMITE CUBIC INTERPOLANT VIA DIVIDED DIFFERENCES.
C
        DD01(P0,P1,DELS) = (P1-P0)/DELS
        DD001(P0,PP0,P1,DELS) = (DD01(P0,P1,DELS)-PP0)/DELS
        DD011(P0,P1,PP1,DELS) = (PP1-DD01(P0,P1,DELS))/DELS
        DD0011(P0,PP0,P1,PP1,DELS) = (DD011(P0,P1,PP1,DELS) -
     $                                DD001(P0,PP0,P1,DELS))/DELS
        QOFS(P0,PP0,P1,PP1,DELS,S) = ((DD0011(P0,PP0,P1,PP1,DELS)*
     $    (S-DELS) + DD001(P0,PP0,P1,DELS))*S + PP0)*S + P0
C
C ***** FIRST EXECUTABLE STATEMENT *****
C
C
C ***** INITIALIZATION *****
C
C LITFH = MAXIMUM NUMBER OF NEWTON ITERATIONS ALLOWED.
C
        ONE = 1.0
        TWOU = 2.0*D1MACH(4)
        FOURU = TWOU + TWOU
        NP1 = N+1
        FAILED = .FALSE.
        CRASH = .TRUE.
        LITFH = 10
        PCGWK = 2*N+3
        ZU = 3*N+4
C
C CHECK THAT ALL INPUT PARAMETERS ARE CORRECT.
C
C     THE ARCLENGTH  S  MUST BE NONNEGATIVE.
C
        IF (S .LT. 0.0) RETURN
C
C     IF STEP SIZE IS TOO SMALL, DETERMINE AN ACCEPTABLE ONE.
C
        IF (H .LT. FOURU*(1.0+S)) THEN
          H=FOURU*(1.0 + S)
          RETURN
        END IF
C
C     IF ERROR TOLERANCES ARE TOO SMALL, INCREASE THEM TO ACCEPTABLE
C     VALUES.
C
        TEMP=DNRM2(NP1,Y,1) + 1.0
        IF (.5*(RELERR*TEMP+ABSERR) .LT. TWOU*TEMP) THEN
          IF (RELERR .NE. 0.0) THEN
            RELERR = FOURU*(1.0+FOURU)
            TEMP = 0.0
            ABSERR = MAX(ABSERR,TEMP)
          ELSE
            ABSERR=FOURU*TEMP
          END IF
          RETURN
        END IF
C
C     INPUT PARAMETERS WERE ALL ACCEPTABLE.
C
        CRASH = .FALSE.
C
C COMPUTE  YP  ON FIRST CALL.
C
        IF (START) THEN
C
C         INITIALIZE THE IDEAL ERROR USED FOR STEP SIZE ESTIMATION.
C
          IDLERR=SQRT(SQRT(ABSERR))
C
C         INITIALIZE STARTING POINTS FOR THE CONJUGATE GRADIENT
C         ALGORITHM TO ZERO.
C
          DO 10 J=1,2*N+2
            WORK(J)=0.0
  10      CONTINUE
          CALL TANGQS(Y,YP,YPOLD,A,QR,PIVOT,PP,RHOVEC,WORK,
     $              N,LENQR,IFLAG,NFE,PAR,IPAR)
          IF (IFLAG .GT. 0) RETURN
        END IF
C
C ***** COMPUTE PREDICTOR POINT Z0 *****
C
  20    IF (START) THEN
C
C         COMPUTE Z0 WITH LINEAR PREDICTOR USING Y, YP --
C         Z0 = Y+H*YP.
C
          CALL DCOPY(NP1,Y,1,Z0,1)
          CALL DAXPY(NP1,H,YP,1,Z0,1)
C
        ELSE
C
C         COMPUTE Z0 WITH CUBIC PREDICTOR.
C
          DO 30 I=1,NP1
            Z0(I) = QOFS(YOLD(I),YPOLD(I),Y(I),YP(I),HOLD,HOLD+H)
  30      CONTINUE
C
        END IF
C
C ***** END OF PREDICTOR SECTION *****
C
C ***** NEWTON ITERATION *****
C
        DO 140 ITCNT = 1,LITFH
C
C SET STARTING POINTS FOR CONJUGATE GRADIENT ALGORITHM.
C
        DO 40 J=ZU,ZU+2*N+1
          WORK(J) = 0.0
  40    CONTINUE
C
C COMPUTE QR = [D RHO/DX], RHOVEC=RHO, -PP= (D RHO/D LAMBDA).
C
        LAMBDA = Z0(NP1)
        IF (IFLAG .EQ. -2) THEN
C
C         CURVE TRACKING PROBLEM.
C
          CALL RHOJS(A,LAMBDA,Z0,QR,LENQR,PIVOT,PP,PAR,IPAR)
          CALL RHO(A,LAMBDA,Z0,RHOVEC,PAR,IPAR)
        ELSE IF (IFLAG .EQ. -1) THEN
C
C         ZERO FINDING PROBLEM.
C
          CALL FJACS(Z0,QR,LENQR,PIVOT)
          CALL DSCAL(LENQR,LAMBDA,QR,1)
          SIGMA=1.0-LAMBDA
          DO 50 J=1,N
            QR(PIVOT(J))=QR(PIVOT(J))+SIGMA
  50      CONTINUE
          CALL DCOPY(N,Z0,1,RHOVEC,1)
          CALL DAXPY(N,-ONE,A,1,RHOVEC,1)
          CALL F(Z0,PP)
          CALL DSCAL(N,-ONE,PP,1)
          CALL DAXPY(N,ONE,RHOVEC,1,PP,1)
          CALL DAXPY(N,-LAMBDA,PP,1,RHOVEC,1)
        ELSE
C
C         FIXED POINT PROBLEM.
C
          CALL FJACS(Z0,QR,LENQR,PIVOT)
          CALL DSCAL(LENQR,-LAMBDA,QR,1)
          DO 60 J=1,N
            QR(PIVOT(J)) = QR(PIVOT(J))+1.0
  60      CONTINUE
          CALL DCOPY(N,Z0,1,RHOVEC,1)
          CALL DAXPY(N,-ONE,A,1,RHOVEC,1)
          CALL F(Z0,PP)
          CALL DAXPY(N,-ONE,A,1,PP,1)
          CALL DAXPY(N,-LAMBDA,PP,1,RHOVEC,1)
        END IF
        RHOVEC(NP1) = 0.0
        NFE = NFE+1
C
C SOLVE SYSTEM TO FIND NEWTON STEP  DZ.
C
          CALL PCGQS(N,QR,LENQR,PIVOT,PP,YP,RHOVEC,DZ,
     $         WORK(PCGWK),IFLAG)
          IF (IFLAG .GT. 0) RETURN
C
C TAKE STEP.
C
          CALL DAXPY(NP1, ONE, DZ, 1, Z0, 1)
C
C CHECK FOR CONVERGENCE.
C
          XSTEP=DNRM2(NP1,DZ,1)
          IF (XSTEP .LE. RELERR*DNRM2(NP1,Z0,1)+ABSERR) THEN
             GO TO 160
          END IF
C
  140   CONTINUE
C
C ***** END OF NEWTON LOOP *****
C
C ***** DIDN'T CONVERGE OR TANGENT AT NEW POINT DID NOT MAKE *****
C       AN ANGLE SMALLER THAN 60 DEGREES WITH  YPOLD --
C       TRY AGAIN WITH A SMALLER H
C
  150   FAILED = .TRUE.
        HFAIL = H
        IF (H .LE. FOURU*(1.0 + S)) THEN
          IFLAG = 6
          RETURN
        ELSE
          H = .5 * H
        END IF
        GO TO 20
C
C ***** END OF CONVERGENCE FAILURE SECTION *****
C
C ***** CONVERGED -- MOP UP AND RETURN *****
C
C COMPUTE TANGENT AT Z0.
C
  160   CALL TANGQS(Z0,T,YP,A,QR,PIVOT,PP,RHOVEC,WORK,N,
     $       LENQR,IFLAG,NFE,PAR,IPAR)
        IF (IFLAG .GT. 0) RETURN
C
C CHECK THAT COMPUTED TANGENT  T  MAKES AN ANGLE NO LARGER THAN
C 60 DEGREES WITH CURRENT TANGENT  YP.  (I.E., COS OF ANGLE < .5)
C IF NOT, STEP SIZE WAS TOO LARGE, SO THROW AWAY Z0, AND TRY
C AGAIN WITH A SMALLER STEP.
C
        ALPHA = DDOT(NP1,T,1,YP,1)
        IF (ALPHA .LT. 0.5) GOTO 150
        ALPHA = ACOS(ALPHA)
C
C COMPUTE CORRECTOR DISTANCE.
C
      IF (START) THEN
        CALL DCOPY(NP1,Y,1,WORK(PCGWK),1)
        CALL DAXPY(NP1,H,YP,1,WORK(PCGWK),1)
      ELSE
        DO 170 I=1,NP1
          WORK(PCGWK+I-1)=QOFS(YOLD(I),YPOLD(I),Y(I),YP(I),HOLD,HOLD+H)
  170   CONTINUE
      ENDIF
        CALL DAXPY(NP1,-ONE,Z0,1,WORK(PCGWK),1)
        CORDIS=DNRM2(NP1,WORK(PCGWK),1)
C
C SET UP VARIABLES FOR NEXT CALL.
C
        CALL DCOPY(NP1,Y,1,YOLD,1)
        CALL DCOPY(NP1,Z0,1,Y,1)
        CALL DCOPY(NP1,YP,1,YPOLD,1)
        CALL DCOPY(NP1,T,1,YP,1)
C
C UPDATE ARCLENGTH   S = S + ||Y-YOLD||.
C
        HTEMP = HOLD
        CALL DAXPY(NP1,-ONE,YOLD,1,Z0,1)
        HOLD = DNRM2(NP1,Z0,1)
        S = S+HOLD
C
C COMPUTE IDEAL ERROR FOR STEP SIZE ESTIMATION.
C
        IF (ITCNT .LE. 1) THEN
          THETA = 8.0
        ELSE IF (ITCNT .EQ. 4) THEN
          THETA = 1.0
        ELSE
          OMEGA=XSTEP/CORDIS
          IF (ITCNT .LT. 4) THEN
            LK = 4*ITCNT-7
            IF (OMEGA .GE. WRGE(LK)) THEN
              THETA = 1.0
            ELSE IF (OMEGA .GE. WRGE(LK+1)) THEN
              THETA = ACOF(LK) + ACOF(LK+1)*LOG(OMEGA)
            ELSE IF (OMEGA .GE. WRGE(LK+2)) THEN
              THETA = ACOF(LK+2) + ACOF(LK+3)*LOG(OMEGA)
            ELSE
              THETA = 8.0
            END IF
          ELSE IF (ITCNT .GE. 7) THEN
            THETA = 0.125
          ELSE
            LK = 4*ITCNT - 16
            IF (OMEGA .GT. WRGE(LK)) THEN
              LST = 2*ITCNT - 1
              THETA = ACOF(LST) + ACOF(LST+1)*LOG(OMEGA)
            ELSE
              THETA = 0.125
            END IF
          END IF
        END IF
        IDLERR=THETA*IDLERR
C
C     IDLERR SHOULD BE NO BIGGER THAN 1/2 PREVIOUS STEP.
C
        IDLERR = MIN(.5*HOLD,IDLERR)
C
C COMPUTE OPTIMAL STEP SIZE.
C   WK = APPROXIMATE CURVATURE = 2*SIN(ALPHA/2)/HOLD  WHERE
C        ALPHA = ARCCOS(YP*YPOLD).
C   GAMMA = EXPECTED CURVATURE FOR NEXT STEP, COMPUTED BY
C        EXTRAPOLATING FROM CURRENT CURVATURE  WK, AND LAST
C        CURVATURE  WKOLD.  GAMMA  IS FURTHER REQUIRED TO BE
C        POSITIVE.
C
        WKOLD = WK
        WK = 2.0*ABS(SIN(.5*ALPHA))/HOLD
        IF (START) THEN
           GAMMA = WK
        ELSE
           GAMMA = WK + HOLD/(HOLD+HTEMP)*(WK-WKOLD)
        END IF
        GAMMA = MAX(GAMMA, 0.01*ONE)
        H = SQRT(2.0*IDLERR/GAMMA)
C
C     ENFORCE RESTRICTIONS ON STEP SIZE SO AS TO ENSURE STABILITY.
C        HMIN <= H <= HMAX, BMIN*HOLD <= H <= BMAX*HOLD.
C
        H = MIN(MAX(SSPAR(1),SSPAR(3)*HOLD,H),SSPAR(4)*HOLD,SSPAR(2))
        IF (FAILED) H = MIN(HFAIL,H)
        START = .FALSE.
C
C ***** END OF MOP UP SECTION *****
C
        RETURN
C
C ***** END OF SUBROUTINE STEPQS *****
        END

.