C        **********************************************************
C        *                THE HATFIELD POLYTECHNIC                *
C        *              NUMERICAL OPTIMISATION CENTRE             *
C        *                  THE  OPTIMA90  PACKAGE                *
C        *       DOUBLE PRECISION  SUBROUTINE:   OPALQP           *
C        *              Fortran 90  VERSION 1,  July 1995         *
C        **********************************************************
C
C****************************************************************************
C*                                                                          *
C*  MODIFICATION HISTORY:                                                   *
C*                                                                          *
C*  DATE      INITIALS  LINE  COMMENTS                                      *
C*                                                                          *
C*  26:01:89  MBB       0     First version                                 *
C*  14:07:95  MBB             Simple changes interface with OPFAD
C*                            No other f90 improvements
C*                            Does not handle numeric derivatives yet
C*  24:07:95  MBB             Improved positive definite fixup
C*                            changing y when when py is negative
C*                            and correcting theta factor from tr208
C*  25:07:95  MBB             Option of diagonal update or usual bfgs
C****************************************************************************
C
C
C                               OPALQP
C                               -----
C The subroutine package OPALQP is based on similar ideas to the earlier
C OPTIMA codes OPRQP and OPXRQP but also embodies some important changes,
C most of which do not affect the way in which the package is used.
C Specifically the improvements are:
C * The use of an INEQUALITY constrained subproblem for computing search
C   directions.  This subproblem is based on the properties of the
C   the augmented Lagrangian function: so also is the line search.
C * Extra safeguards on penalty parameter reduction in order to ensure
C   global convergence (see NOC T.R. 168).
C * The use of a different penalty parameter for each constraint.
C * Extra safeguards in the matrix updating to avoid ill-CONDitioning.
C
C  PURPOSE: TO FIND THE MINIMUM OF A FUNCTION OF SEVERAL VARIABLES
C  SUBJECT TO EQUALITY AND INEQUALITY CONSTRAINTS.
C
C  USAGE:CALL OPALQP (N,M,ME,X,FF,F,B1,G,A,U,IV,IVSET,R1,SCALE,
C                    EPS,IPRINT,IMAX,Q)
C
C  WHERE THE ARGUMENTS ARE AS FOLLOWS:
C
C  N = THE NUMBER OF INDEPENDENT VARIABLES.
C
C  M = THE TOTAL NUMBER OF CONSTRAINTS.
C
C  ME = THE NUMBER OF EQUALITY CONSTRAINTS. NOTE THAT THE ORDERING
C    OF THE CONSTRAINTS MUST BE SUCH THAT THE FIRST ME CONSTRAINTS
C    ARE THE EQUALITIES.
C
C  X = AN N-VECTOR OF VALUES FOR THE VARIABLES. AT ENTRY TO OPALQP
C    X MUST CONTAIN AN ESTIMATE OF THE POSITION OF THE MINIMUM AND
C    AT EXIT THIS VECTOR WILL CONTAIN THE BEST VALUES FOUND FOR THE
C    VARIABLES.
C
C  FF = THE BEST VALUE FOUND FOR THE FUNCTION AT EXIT FROM OPALQP.
C
C  F = AN N-VECTOR CONTAINING THE FIRST DERIVATIVES OF THE FUNCTION
C    AT EXIT FROM OPALQP.
C
C  B1 = AN N-BY-N MATRIX USED TO BUILD AN ESTIMATE OF THE INVERSE
C    HESSIAN MATRIX OF THE LAGRANGIAN FUNCTION.  SOME VALUES MUST BE
C    GIVEN TO THE ELEMENTS OF B1 BEFORE ENTRY TO OPALQP, BUT IN THE
C    ABSENCE OF ANY BETTER INFORMATION B1 CAN BE SET TO THE IDENTITY
C    MATRIX.  THE ONLY RESTRICTION IS THAT B1 MUST BE A POSITIVE
C    DEFINITE MATRIX.
C
C  A = AN M-BY-N MATRIX CONTAINING THE FIRST DERIVATIVES OF THE
C    CONSTRAINT EXPRSSIONS. SPECIFICALLY A(I,J) IS THE DERIVATIVE
C    OF THE I-TH CONSTRAINT W.R.T. THE J-TH VARIABLE.
C
C  U = AN M-VECTOR WHICH AT EXIT CONTAINS ESTIMATES OF THE LAGRANGE
C    MULTIPLIERS ASSOCIATED WITH THE CONSTRAINTS.
C
C  IV = AN INTEGER WHICH IS THE NUMBER OF CONSTRAINTS WHICH ARE ACTIVE
C    (I.E. WHICH ARE VIOLATED, OR HAVE ZERO VALUE) AT THE FINAL POINT.
C
C  IVSET = AN M-VECTOR OF INTEGERS, WHOSE FIRST IV ELEMENTS CONTAIN
C    THE INDICES OF THE ACTIVE CONSTRAINTS.
C
C  R = AN M-VECTOR OF PENALTY PARAMETERS, ONE FOR EACH CONSTRAINT.
C    INITIAL VALUES FOR ALL THE ELEMENTS OF R MUST BE PROVIDED BEFORE
C    ENTRY TO OPALQP.  THE ROUTINE ADJUSTS THESE VALUES AS IT PROCEEDS.
C
C  SCALE = A PARAMETER (LESS THAN 1) SET BY THE USER WHICH GOVERNS
C    THE RATE AT WHICH THE PENALTY PARAMETERS ARE REDUCED TOWARDS ZERO.
C
C  EPS = AN (M+1)-VECTOR OF CONVERGENCE PARAMETERS. THE ELEMENTS
C    OF EPS ARE SPECIFIED BY THE USER SO THAT A SATISFACTORY
C    SOLUTION IS DEFINED BY ALL EQUALITY CONSTRAINTS BEING SUCH
C    THAT EPS(I)>G(I)>-EPS(I); ALL INEQUALITY CONSTRAINTS BEING SUCH
C    THAT G(I)>-EPS(I); AND THE PROJECTED GRADIENT BEING LESS THAN
C    EPS(M+1).
C
C  IPRINT = A USER-SPECIFIED PARAMETER GOVERNING OUTPUT. VARIABLES,
C    FUNCTION VALUE, CONSTRAINTS AND LAGRANGE MULTIPLIER ESTIMATES
C    ARE PRINTED EVERY IPRINT ITERATIONS. IPRINT=0 SUPPRESSES ALL
C    OUTPUT EXCEPT THAT AT THE FINAL POINT.  NEGATIVE  IPRINT SUPRESSES
C    ALL THE OUTPUT.
C
C  IMAX = A USER-SPECIFIED PARAMETER GIVING THE MAXIMUM NUMBER OF
C    ITERATIONS THAT MAY BE PERFORMED.
C
C  Q = A WORKSPACE VECTOR WHICH MUST BE DIMENSIONED IN THE CALLING
C   PROGRAM TO HAVE LENGTH AT LEAST M*MAX(M,N)
C
C  THE CONTROL PROGRAM CALLING OPALQP MUST CORRECTLY DIMENSION THE
C  VECTORS X AND F TO LENGTH N; THE VECTORS G,U AND IVSET TO LENGTH
C  M; THE VECTOR EPS TO LENGTH (M+1) AND THE MATRICES B1 AND A
C  AS N-BY-N AND M-BY-N RESPECTIVELY.
C
C  FOR PROBLEMS ABOVE A CERTAIN SIZE SOME EXTRA WORKSPACE HAS TO
C  BE MADE AVAILABLE THROUGH NAMED COMMON AREAS. SPECIFICALLY IF
C  M>20 THEN THE FOLLOWING STATEMENTS ARE NEEDED:
C    COMMON/W1/C(M)
C    COMMON/W4/DIAG(M)  *************************!!!!
C    COMMON/W7/V(M)
C    COMMON/W8/Z(M)
C    COMMON/W9/G1(M)
C    COMMON/W10/G2(M).
C  IF N>20 THEN THE FOLLOWING ARE REQUIRED:
C    COMMON/W2/P(N)
C    COMMON/W3/OLDF(N)
C    COMMON/W4/DIAG(N)
C    COMMON/W5/BETA(N)
C    COMMON/W6/ATG(N)
C    COMMON/W11/Z1(N)
C    COMMON/W20/BX(N)
C    COMMON/W30/BP(N).
C  IN THESE COMMON STATEMENTS IT IS THE NUMERICAL VALUE OF N (OR M)
C  THAT MUST APPEAR.  IT IS NOT THE VARIABLE NAMES THAT ARE CRUCIAL
C  BUT THE COMMON AREA NAMES MUST BE USED PRECISELY.
C                            ----
C
C  THE USER MUST SUPPLY A SUBROUTINE ENTITLED CALFUN(X,N,M,FF,G)
C  WHICH CALCULATES THE FUNCTION VALUE FF AND CONSTRAINT VALUES
C  G(1)...G(M) FOR ANY VALUES OF THE VARIABLES X(1)...X(N). THE
C  USER MAY ALSO SUPPLY A SUBROUTINE CALGRD(X,N,M,FF,F,G,A) WHICH
C  CALCULATES THE GRADIENT OF THE FUNCTION AS F(1)...F(N) AND
C  THE FIRST DERIVATIVES OF THE CONSTRAINTS AS THE MATRIX A(I,J).
C  A LIBRARY SUBROUTINE EXISTS WHICH ESTIMATES DERIVATIVES BY
C  DIFFERENCES AND THIS MAY BE USED IF ANALYTIC GRADIENTS ARE
C  NOT READILY AVAILABLE.  FOR DETAILS OF THIS SUBROUTINE (CALLED
C  OPND3) SEE THE USER NOTE.
C
C
C  AN OUTPUT SWITCH IS AVAILABLE IN COMMON AREA  STOPSW.
C  THIS INTEGER SWITCH DESCRIBES THE OUTPUT RESULT.
C  IT WILL ON EXIT CONTAIN ONE OF THE VALUES:
C
C           1   CONVERGENCE
C           3   MORE THAN  IMAX  ITERATIONS NEEDED
C           5   NO FURTHER REDUCTION IN FUNCTION OR CONSTRAINTS
C           6   WRONG NUMERICAL DERIVATIVE SUBROUTINE
C          14   LAGRANGE MULTIPLIER EQUATIONS SINGULAR
C          15   SEARCH DIRECTION UPHILL.
C
      SUBROUTINE OPALQP(N,M,ME,X,FF,F,H,G,A,U,IV,IVSET,R,
     1SCALE,EPS,IPRINT,IMAX,Q)
C  MAIN CALCULATIONS
      IMPLICIT DOUBLE PRECISION(A-H,O-Z)
      DIMENSION X(1),F(1),G(1),U(1),IVSET(1),EPS(1),R(1)
      DIMENSION H(N,N),A(M,N),Q(M,1)
      COMMON/W7/V(20)
      COMMON/W2/P(20)/W3/GRADP(20)/W5/GRADL1(20)
      COMMON/W6/GRADL2(20)
      COMMON/XI/V1,V2
      COMMON/DEVICE/IOC
      COMMON/HMAT/MATRIX
      COMMON/STOPSW/ISTOP
      common/which/iup
      Interface
        Subroutine Calfun(x,n,m,ff,g,f,A)
        Integer, Intent(in) :: n,m
        Real (Kind(1.0d0)), Intent(in) :: x(n)
        Real (Kind(1.0d0)), Intent(out) :: ff, g(m)
        Real (KInd(1.0d0)), Optional, Intent(out) :: f(n), A(m,n)
        End subroutine Calfun
      End interface
      write(6,*)'which update'
      write(6,*)'0 = standard BFGS'
      write(6,*)'1 = diagonal'
      read(5,*)iup
      ICT=0
      KOUT=0
      KINN=0
      ISTOP=0
      V3=1.0
      INEXT=IPRINT
      DO 1100 I=1,M
        U(I)=0.0
        V(I)=U(I)
1100  CONTINUE
      ISGRAD=1
      IFV=1
      IGV=1
      Call Calfun(x,n,m,ff,g,f,A)
      IF(ISTOP.EQ.6) THEN
        IF(IPRINT.GT.0) CALL OUTPUT(N,M,X,FF,G,U,V,R,IFV,IGV,ISTOP,IOC)
        RETURN
      END IF
      IF(IPRINT.GT.0) CALL VALUES
      CALL LAGGRD(N,M,ME,F,G,A,U,GRADL2)
      CALL PENGRD(N,M,ME,F,G,A,V,R,GRADP)
      GP0=SQRT(DOTV(N,GRADP,GRADP))
      GL0=SQRT(DOTV(N,GRADL2,GRADL2))
      GTG0=SQRT(VTV(M,ME,V,R,G))
      CTC0=SQRT(ATA(M,ME,U,G))
      TEST0=V3*GL0+CTC0
      TEST=TEST0
      GL=GL0
      GTG=GTG0
      GP=GP0
      IF(IPRINT.GT.0) WRITE(IOC,3011)KINN,KOUT,TEST,GL,GTG,GP
      IF(IPRINT.GT.0) CALL OUTPUT(N,M,X,FF,G,U,V,R,IFV,IGV,ISTOP,IOC)
C  START OF ITERATION
2100  ICT=ICT+1
      IF(ICT.GT.IMAX) THEN
        ISTOP=3
        IF(IPRINT.LT.0) RETURN
        WRITE(IOC,3011)KINN,KOUT,TEST,GL,GTG,GP
        CALL OUTPUT(N,M,X,FF,G,U,V,R,IFV,IGV,ISTOP,IOC)
        RETURN
      END IF
      CALL IQP(N,M,ME,IV,IVSET,F,H,G,A,Q,U,V,R,P,ISTOP)
      IF(ISTOP.EQ.14) THEN
        IF(IPRINT.LT.0) RETURN
        WRITE(IOC,3011)KINN,KOUT,TEST,GL,GTG,GP
        CALL OUTPUT(N,M,X,FF,G,U,V,R,IFV,IGV,ISTOP,IOC)
        RETURN
      END IF
      ISTOP=0
      CALL LAGGRD(N,M,ME,F,G,A,U,GRADL1)
      CALL PENFUN(FF,M,ME,G,V,R,PFF)
      CALL PENGRD(N,M,ME,F,G,A,V,R,GRADP)
      PGRADP=DOTV(N,P,GRADP)
C   CHECK FOR UPHILL SEARCH DIRECTION
      IF(PGRADP.GE.0.0) THEN
        IF(ISGRAD.EQ.0) THEN
          CALL UNIT(H,N)
          ISGRAD=1
          ICT=ICT-1
          GOTO 2100
         ELSE
          ISTOP=15
          IF(IPRINT.LT.0) RETURN
          WRITE(IOC,3011)KINN,KOUT,TEST,GL,GTG,GP
          CALL OUTPUT(N,M,X,FF,G,U,V,R,IFV,IGV,ISTOP,IOC)
          RETURN
        END IF
      END IF
      SC=1.0
      KINN=ICT
      CALL SEARCH(N,M,ME,X,P,FF,G,R,PFF,PGRADP,SC,IFV,ISTOP)
      IF(ISTOP.EQ.5) THEN
        IF(ISGRAD.EQ.0) THEN
          CALL UNIT(H,N)
          ISGRAD=1
          ICT=ICT-1
          GOTO 2100
         ELSE
          IF(IPRINT.LT.0) RETURN
          WRITE(IOC,3011)KINN,KOUT,TEST,GL,GTG,GP
          CALL OUTPUT(N,M,X,FF,G,U,V,R,IFV,IGV,ISTOP,IOC)
          RETURN
        END IF
      END IF
      IF(MATRIX.EQ.1) CALL PENGRD(N,M,ME,F,G,A,V,R,GRADL1)
      Call Calfun(x,n,m,ff,g,f,A)
      IGV=IGV+1
      CALL PENGRD(N,M,ME,F,G,A,V,R,GRADP)
      GP=SQRT(DOTV(N,GRADP,GRADP))
      CALL LAGGRD(N,M,ME,F,G,A,V,GRADL2)
      GL0=SQRT(DOTV(N,GRADL2,GRADL2))
      CALL LAGGRD(N,M,ME,F,G,A,U,GRADL2)
      GL=SQRT(DOTV(N,GRADL2,GRADL2))
      GTG=SQRT(VTV(M,ME,V,R,G))
      CTC=SQRT(ATA(M,ME,U,G))
      TEST=V3*GL+CTC
      U2=0.0
      DO 4100 I=1,M
        IF(I.GT.ME.AND.G(I).GT.0.0.AND.U(I).LE.0.0) GOTO 4100
        U2=U2+(R(I)*(U(I)-V(I)))**2
4100  CONTINUE
      GPRED=0.5*SQRT(U2)
      CALL CONVRG(N,M,ME,F,GRADL2,G,U,EPS,ISTOP)
      IF(ISTOP.EQ.1) THEN
        IF(IPRINT.LT.0) RETURN
        WRITE(IOC,3011)KINN,KOUT,TEST,GL,GTG,GP
        CALL OUTPUT(N,M,X,FF,G,U,V,R,IFV,IGV,ISTOP,IOC)
        RETURN
      END IF
      IF(MATRIX.EQ.1) CALL PENGRD(N,M,ME,F,G,A,V,R,GRADL2)
      CALL UPDATE(H,N,SC,P,GRADL1,GRADL2)
      ISGRAD=0
      IF(ICT.EQ.INEXT) THEN
        INEXT=INEXT+IPRINT
        WRITE(IOC,3011)KINN,KOUT,TEST,GL,GTG,GP
        CALL OUTPUT(N,M,X,FF,G,U,V,R,IFV,IGV,ISTOP,IOC)
      END IF
C  TESTS FOR REVISING R AND V
      IF(TEST.GT.V1*TEST0.AND.GP.GT.V2*GTG) GOTO 2100
      IF(GP.GT.V2*GTG.AND.GPRED.LT.0.9*CTC.AND.GL.GE.GL0) GOTO 2100
      IF(GP.LE.V2*GTG.OR.GPRED.GT.0.9*CTC) CALL PENLTY(M,ME,G,U,V,
     *SCALE,R)
      IF(GL.LT.GL0) THEN
        DO 5100 I=1,M
          IF(I.LE.ME)V(I)=U(I)
          IF(I.GT.ME.AND.U(I).GT.0.0) V(I)=U(I)
          IF(I.GT.ME.AND.U(I).LE.0.0) V(I)=0.0
5100    CONTINUE
      END IF
      KOUT=KOUT+1
      IF(IPRINT.GT.0) WRITE(IOC,3011)KINN,KOUT,TEST,GL,GTG,GP
      TEST0=DMIN1(TEST,TEST0)
      GOTO 2100
3011  FORMAT(/,' OPTIMALITY TESTS AFTER',I4,' INNER(',I4,' OUTER)',
     1   '  ITERATIONS',/,'   K-T=',1PD7.0,';  GRADL=',1PD7.0,
     2   ';   GTG=',1PD7.0,';  GRADP=',1PD7.0)
      END
C
      SUBROUTINE EQP(N,M,IV,IVSET,F,H,C,A,Q,U,V,R,P,BP,ISTOP)

C  Calculates Lagrange multipliers u and search direction d to solve EQP
C  based on the active constraints
C  Min (p'Bp)/2 + f'p  s.t.  c + Ap = -R(u-v)/2
C  Note that c, A, u and v in this subroutine usually consist of
C  only some of the rows of c, A, u and v from subroutine IQP
 
      IMPLICIT DOUBLE PRECISION(A-H,O-Z)
      DIMENSION IVSET(M),F(N),H(N,N),C(M),A(M,N),U(M),Q(M,M)
      DIMENSION P(N),BP(N),V(M),R(M)
C      COMMON/W1/RHS(20)
      COMMON/W9/Y(20)
      COMMON/W4/DIAG(20)/W11/Z(20)
C      COMMON/W6/GRADL(20)
C  Compute p = -Hf and Bp = -f

      DO 1100 I=1,N
        P(I)=0.0D0
        BP(I) = -F(I)
        DO 1100 J=1,N
         P(I) = P(I) - H(I,J)*F(J)     
1100  CONTINUE
C
      IF(IV.EQ.0) RETURN
C  Form rhs vector z = AHf - g + rv/2
      DO 1301 I=1,IV
        II=IVSET(I)
        Z(I)=0.0
        DO 1300 J=1,N
         Z(I) = Z(I) - A(II,J)*P(J)
1300  CONTINUE
        Z(I) = Z(I) - C(II) + 0.5*R(II)*V(II)
1301  CONTINUE
C  FORM THE MATRIX Q = (RI/2 + AHA')
      DO 1600 I=1,IV
        II=IVSET(I)
        DO 1600 J=1,N
          QIJ=0.
          DO 1500 K=1,N
            QIJ=QIJ+A(II,K)*H(K,J)
1500      CONTINUE
          Q(I,J)=QIJ
1600  CONTINUE
      DO 2500 I=1,IV
        II=IVSET(I)
        DO 2300 J=I,IV
          IJ=IVSET(J)
          Y(J)=0.
          DO 2300 K=1,N
            Y(J)=Y(J)+Q(I,K)*A(IJ,K)
2300    CONTINUE
        DO 2500 J=I,IV
          Q(I,J)=Y(J)
          IF(I.EQ.J)Q(I,J)=Q(I,J)+0.5D0*R(II)
2500  CONTINUE
C
      ISING=0
      CALL DECOMP(M,IV,Q,DIAG,ISING)
      IF(ISING.EQ.1) THEN
        ISTOP=14
        RETURN
      END IF
      CALL SOLVE(M,IV,Q,Z,Y,DIAG)
C Enter new elements in vector u
      DO 3300 I=1,IV
        II=IVSET(I)
        U(II) = Y(I)
3300  CONTINUE
C Compute Bp = -(f-A'u)
      DO 4300 I=1,N
         BP(I) = -F(I)
        DO 4100 J=1,IV
          IJ=IVSET(J)
          BP(I) = BP(I) + A(IJ,I)*U(IJ)
4100    CONTINUE
4300  CONTINUE
C Compute p = -H(f-A'u)
      DO 4500 I=1,N
          P(I) = 0.0D0
        DO 4500 K=1,N
          P(I) = P(I) + H(I,K)*BP(K)
4500  CONTINUE
C  Computation of scalar product p'(f - A'v + 2A'c/r)
C  This should be negative as it is the directional derivative
C  of the augmented Lagrangian
      PGRADL = 0.0D0
      DO 4501 I = 1,N
        Z(I) = F(I)
        DO 4502 J = 1,IV
         IJ = IVSET(J)
         Z(I) = Z(I) - A(IJ,I)*V(IJ) + 2.0D0*A(IJ,I)*C(IJ)/R(IJ)
4502  CONTINUE
      PGRADL = PGRADL + P(I)*Z(I)
4501  CONTINUE
     
      RETURN
      END
C
      SUBROUTINE IQP(N,M,ME,IV,IVSET,B,H,C,A,Q,U,V,R,P,ISTOP)
C  Solves the QP
C  Min (p'Bp)/2 + b'p   S.T. c+Ap > -R(u-v)/2
C  where matrix variable H is inverse of B
      IMPLICIT DOUBLE PRECISION (A-H,O-Z)
      DIMENSION P(1),B(1),C(1),U(1),IVSET(1),V(1),R(1)
      DIMENSION H(N,N),A(M,N),Q(M,M)
      COMMON/W3/F(20)/W5/D(20)/W6/GRADL(20)/W8/G(20)
      COMMON/W20/BX(20)/W30/BP(20)
      COMMON/QPIT/IMAX

      ICT=0
      ISTOP=0
      IPRINT=1
      INEXT=IPRINT
      DO 1100 I=1,N
        P(I)=0.0
        BX(I)=0.0
        F(I)=B(I)
1100  CONTINUE
      DO 1300 I=1,M
        G(I)=C(I)
1300     CONTINUE

C  START OF ITERATION
2100  ICT=ICT+1
      IF(ICT.GE.IMAX) ISTOP=3
      CALL ACTIVE(M,ME,G,U,V,IV,IVSET,R)

      CALL EQP(N,M,IV,IVSET,F,H,C,A,Q,U,V,R,P,BP,ISTOP)
      IF(ISTOP.EQ.14) GOTO 5100

      CALL PROBE(N,M,ME,C,A,U,V,P,R,SC)
      DO 3100 I=1,N
        P(I)=SC*P(I)
        BX(I) = SC*BP(I)
3100  CONTINUE

      DO 3300 I=1,M
        G(I)=C(I)
        DO 3300 J=1,N
          G(I)=G(I)+A(I,J)*P(J)
3300  CONTINUE
      IF(SC.GE.1.0) THEN
        DO 3500 I=1,M
          IF(I.GT.ME.AND.U(I).LT.0.0) GOTO 3700
3500    CONTINUE
        ISTOP=1
      END IF
3700  CONTINUE
      DO 4100 I=1,M
        IF(I.GT.ME)U(I)=DMAX1(0D0,U(I))
4100  CONTINUE
      IF(ISTOP.LE.0) THEN
        IF(ICT.NE.INEXT) GOTO 2100
        INEXT=INEXT+IPRINT
      END IF
5100  CONTINUE
      IF(ISTOP.EQ.0.AND.IMAX.GT.0) GOTO 2100

      RETURN
      END
C
      SUBROUTINE PENFUN(FF,M,ME,G,V,R,PFF)
C  CALCULATES PENALTY FUNCTION INCLUDING ALL EQUALITY CONSTRAINTS AND 
C  THOSE INEQUALITIES FOR WHICH G<R*V/2
      IMPLICIT DOUBLE PRECISION(A-H,O-Z)
      DIMENSION G(M),R(M),V(M)
      PFF=0.0
      DO 1000 I=1,M
        GV=G(I)-0.5*R(I)*V(I)
        IF(I.LE.ME.OR.GV.LT.0.0)PFF=PFF+GV**2/R(I)
1000  CONTINUE
      PFF=FF+PFF
      RETURN
      END
C
      SUBROUTINE PENGRD(N,M,ME,F,G,A,V,R,GRADP)
C  CALCULATES PENALTY FUNCTION GRADIENT.
C  PENALTY FUNCTION INCLUDES ALL EQUALITY CONSTRAINTS  PLUS THOSE
C  INEQUALITIES FOR WHICH G<RV/2
      IMPLICIT DOUBLE PRECISION(A-H,O-Z)
      DIMENSION F(N),G(M),A(M,N),V(M),R(M),GRADP(N)
      DO 1300 I=1,N
        GRADP(I)=0.0
        DO 1200 J=1,M
          GV=G(J)-0.5*R(J)*V(J)
          IF(J.LE.ME.OR.GV.LT.0.0)GRADP(I)=GRADP(I)+A(J,I)*GV/R(J)
1200    CONTINUE
        GRADP(I)=F(I)+2.0*GRADP(I)
1300  CONTINUE
      RETURN
      END
C 
      SUBROUTINE ACTIVE(M,ME,G,U,V,IV,IVSET,R)
C  DETERMINES WHICH CONSTRAINTS ARE "ACTIVE", BASED ON VIOLATIONS
C  AND LAGRANGE MULTIPLIER ESTIMATES.
      IMPLICIT DOUBLE PRECISION(A-H,O-Z)
      DIMENSION G(1),IVSET(1),V(1),R(1),U(1)

      IV=0
      DO 1100 I=1,ME
        IV=IV+1
        IVSET(IV)=I
1100  CONTINUE
      DO 1200 I=ME+1,M
        IF(U(I).GT.0.0.OR.G(I).LE.0.5*R(I)*V(I)) THEN
          IV=IV+1
          IVSET(IV)=I
        END IF
1200  CONTINUE
      RETURN
      END
C    
      SUBROUTINE PENLTY(M,ME,G,U,V,SCALE,R)
C  REVISES PENALTY CONSTANTS
      IMPLICIT DOUBLE PRECISION(A-H,O-Z)
      DIMENSION U(1),G(1),V(1),R(1)
      DO 1100 I=1,M
        IF(I.LE.ME.OR.U(I).GT.0.0) R(I)=SCALE*R(I)
1100  CONTINUE
      RETURN
      END
C    
      SUBROUTINE PROBE(N,M,ME,G,A,U,V,P,R,SC)
C  CALCULATES FIRST STEP ALONG P TO CONTROL THE VIOLATION OF NEW CONSTRAINTS.
      IMPLICIT DOUBLE PRECISION(A-H,O-Z)
      DIMENSION G(M),A(M,N),P(N),R(M),U(M),V(M)

      SC=1.0
      DO 1 I=ME+1,M
        IF(G(I).GT.0.5*R(I)*V(I)) THEN
          IF(U(I).EQ.0.0) THEN
            ZZ=0.0
            DO 1100 J=1,N
              ZZ=ZZ+A(I,J)*P(J)
1100        CONTINUE
            IF(ZZ.LT.0.0) THEN
c             sk = (-g(i) + 0.5d0*r(i)+v(i))/zz
              IF(V(I).GT.0.0) SK=-G(I)/ZZ
              IF(V(I).LE.0.0) SK=-(G(I)+0.00001)/ZZ
              IF(SK.LT.SC) SC=SK
            END IF
          END IF
        END IF
1     CONTINUE
      RETURN
      END
C    
      SUBROUTINE CONVRG(N,M,ME,F,GRADL,G,U,EPS,ISTOP)
C  CONVERGENCE TESTS BASED ON GRADIENT OF LAGRANGIAN, CONSTRAINT VIOLATIONS
C  AND SIGNS OF INEQUALITY LAGRANGE MULTIPLIERS.
      IMPLICIT DOUBLE PRECISION(A-H,O-Z)
      DIMENSION F(N),GRADL(N),U(M),G(M),EPS(1)

      DO 1100 I=1,M
        IF(I.LE.ME.AND.ABS(G(I)).GT.EPS(I)) RETURN
        IF(I.GT.ME.AND.U(I).GT.0.0.AND.ABS(G(I)).GT.EPS(I)) RETURN
        IF(I.GT.ME.AND.G(I).LT.-EPS(I)) RETURN
        IF(I.GT.ME.AND.U(I).LT.0.0) RETURN
1100  CONTINUE
      DO 1200 I=1,N
        IF(ABS(GRADL(I)).GT.EPS(M+1)*DMAX1(1D0,ABS(F(I)))) RETURN
1200  CONTINUE
      ISTOP=1
      RETURN
      END
C    
      SUBROUTINE LAGGRD(N,M,ME,F,G,A,U,GRADL)
C  COMPUTES GRADIENT OF LAGRANGIAN FUNCTION INCLUDING ALL EQUALITY 
C  CONSTRAINTS PLUS THOSE INEQUALITIES WITH POSITIVE MULTIPLIERS.
      IMPLICIT DOUBLE PRECISION(A-H,O-Z)
      DIMENSION F(N),G(M),A(M,N),U(M),GRADL(N)
      DO 1300 I=1,N
        GRADL(I)=0.0
        DO 1200 J=1,M
          IF(J.LE.ME.OR.U(J).GT.0.0) GRADL(I)=GRADL(I)-A(J,I)*U(J)
1200    CONTINUE
        GRADL(I)=F(I)+GRADL(I)
1300  CONTINUE
      RETURN
      END
C    
      SUBROUTINE SEARCH(N,M,ME,X,P,FF,G,R,PFF,PGRADP,SC,IFV,ISTOP)
C  LINE SEARCH INVOLVING SAFEGUARDED QUADRATIC INTERPOLATION.
C  EXTRAPOLATION ALSO USED TO MEET WOLFE CONDITIONS.
      IMPLICIT DOUBLE PRECISION(A-H,O-Z)
      DIMENSION X(N),P(N),G(M),R(M)
      COMMON/W11/XNEW(20)/W10/GNEW(20)/W7/V(20)
      COMMON/LSCH/E1,E2/SMALL/STPMIN
      Interface
        Subroutine Calfun(x,n,m,ff,g,f,A)
        Integer, Intent(in) :: n,m
        Real (Kind(1.0d0)), Intent(in) :: x(n)
        Real (Kind(1.0d0)), Intent(out) :: ff, g(m)
        Real (Kind(1.0d0)), Optional, Intent(out) :: f(n), A(m,n)
        End subroutine Calfun      
      End Interface
      SL=0.0
      DL=1.0
      SR=0.0
      DR=0.0
      ITRY=0
C
2100  ISTOP=5
      DF=SC*PGRADP
      DO 1100 I=1,N
        IF(SC*ABS(P(I)).GT.STPMIN*ABS(X(I))) ISTOP=0
        XNEW(I)=X(I)+SC*P(I)
1100  CONTINUE
      IF(ISTOP.EQ.5) RETURN
      Call Calfun(xnew,n,m,ffnew,gnew)

      CALL PENFUN(FFNEW,M,ME,GNEW,V,R,PFFNEW)
      IFV=IFV+1
      D=(PFFNEW-(PFF+DF))/DF + 1.0
      IF(D.LE.E1) THEN
        SR=SC
        DR=D
        FRAC=(DL-0.5)/(DL-DR)
        SC=SL+(SR-SL)*DMAX1(FRAC,1.0D-1)
        DF=SC*PGRADP
        IF (SC==SL) GOTO 2101
        GOTO 2100
      END IF
      IF(ABS(1.0-D).LE.E2.AND.ITRY.LT.5) THEN
        ITRY=ITRY+1
        SL=SC
        DL=D
        IF(SR.GT.0.0)SC=0.5*(SL+SR)
        IF(SR.LE.0.0) THEN
          IF(DL.GE.0.95)SC=10.0*SC
          IF(DL.LT.0.95)SC=0.5*SC/(1.0-DL)
        END IF
        DF=SC*PGRADP
        GOTO 2100
      END IF
2101  DO 1300 I=1,N
        X(I)=XNEW(I)
1300  CONTINUE
      PFF=PFFNEW
      FF=FFNEW
      DO 1500 I=1,M
        G(I)=GNEW(I)
1500  CONTINUE
      RETURN
      END
C
      SUBROUTINE DECOMP(N1,N,Q,DIAG,ISING)
C
C LDL' FACTORIZATION OF SYMMETRIC POSITIVE DEFINITE MATRIX.
C
      IMPLICIT DOUBLE PRECISION (A-H,O-Z)
      DIMENSION DIAG(N1),Q(N1,N1)
      COMMON/LDSING/A8

      ISING=0
      IF(N.EQ.1) THEN
        IF(Q(1,1).EQ.0.0) ISING=1
        RETURN
      END IF
      DO 3100 I=1,N
        DO 3100 J=I,N
          X=Q(I,J)
          IF(I.NE.1) THEN
            II=I-1
            DO 2200 L=1,II
              K=I-L
2200          X=X-Q(J,K)*Q(I,K)
          END IF
          IF(J.GT.I) THEN
            Q(J,I)=X*DIAG(I)
           ELSE
            IF(X.GT.A8*Q(I,I)) THEN
              DIAG(I)=1.D0/SQRT(X)
             ELSE
              write(6,*)'decomp fails'
              write(6,*)i,x,q(i,i)
              ISING=1
              RETURN
            END IF
          END IF
3100  CONTINUE
      RETURN
      END
C
      SUBROUTINE SOLVE (N1,N,Q,RHS,VAR,DIAG)
C
C  SOLUTION OF Q(VAR) = (RHS) USING LDL' FACTORS OF Q.
C
      IMPLICIT DOUBLE PRECISION (A-H,O-Z)
      DIMENSION RHS(N),VAR(N),DIAG(N),Q(N1,N1)

      IF(N.EQ.1) THEN
        VAR(1)=RHS(1)/Q(1,1)
        RETURN
      END IF
      VAR(1)=RHS(1)*DIAG(1)
      DO 1300 I=2,N
        Z=RHS(I)
        II=I-1
        DO 1200 L=1,II
          K=I-L
1200      Z=Z-Q(I,K)*VAR(K)
1300    VAR(I)=Z*DIAG(I)
      VAR(N)=VAR(N)*DIAG(N)
      JJ=N-1
      DO 1500 J=1,JJ
        I=N-J
        Z=VAR(I)
        KK=I+1
        DO 1400 K=KK,N
1400      Z=Z-Q(K,I)*VAR(K)
1500    VAR(I)=Z*DIAG(I)
      RETURN
      END
C   
      SUBROUTINE UPDATE(H,N,SC,P,OLDF,F)
C
C  QUASI-NEWTON UPDATE USING DAMPED VERSION OF BFS FORMULA TO GIVE
C  HY=P WHERE Y=F-OLDF.
C
      IMPLICIT DOUBLE PRECISION(A-H,O-Z)
      DIMENSION H(N,N),P(1),F(1),OLDF(1)
      COMMON/W4/Y(20) /W11/HY(20)
      COMMON/W20/BP(20)
      COMMON/DAMP/TOL /HCOND/COND

      common/which/iup
c     switch to choose between full bfgs and a diagonal update

      TH=1.0
      PY=0.0
      PBP=0.0
      pp = 0.0
      DO 1100 I=1,N
        P(I)=SC*P(I)
        BP(I)=SC*BP(I)
        Y(I)=F(I)-OLDF(I)
        PY=PY+P(I)*Y(I)
        PBP=PBP+P(I)*BP(I)
        pp = pp+ p(i)*p(i)
1100  CONTINUE
      YHY=0.0
      DO 1500 I=1,N
        HY(I)=0.0
        DO 1300 J=1,N
          HY(I)=HY(I)+H(I,J)*Y(J)
1300    CONTINUE
        YHY=YHY+Y(I)*HY(I)
1500  CONTINUE

C  RESET H IF IT APPEARS TO HAVE LOST POSITIVE DEFINITENESS
      IF(YHY.LE.0.0) CALL UNIT(H,N)
      TEST=SQRT(PBP*YHY)

C   DAMPED UPDATE IF PY IS NEGATIVE OR POSITIVE BUT TOO SMALL
      IF(PY.LT.TOL*TEST) THEN
        C=SQRT((PBP/PY)*(YHY/PY)-1.0)*TOL/SQRT(1.0-TOL**2)
        TH=PBP/(PBP+PY*(C-1.0))
c  alternative choice of damping factor
        if(th*py+(1.0-th)*pbp.lt.0.0d0)then
          th=pbp/(pbp+py*(-c-1.0))
        end if
        DO 2100 I=1,N
          HY(I)=TH*HY(I)+(1.0-TH)*P(I)
          Y(I)=TH*Y(I)+(1.0-TH)*BP(I)
2100    CONTINUE
        YHY=TH*TH*YHY+2.0*TH*(1.0-TH)*PY+PBP*(1.0-TH)**2
        PY=TH*PY+(1.0-TH)*PBP
      END IF
      if (iup.eq.0) then
      DO 3100 I=1,N
        DO 3100 J=I,N
          CBFS=YHY*P(I)*P(J)/PY-(HY(I)*P(J)+HY(J)*P(I))
          CBFS=(P(I)*P(J)+CBFS)/PY
          H(I,J)=H(I,J)+CBFS
          H(J,I)=H(I,J)
3100  CONTINUE
      else
        do 3101 i=1,n
          if(p(i)*y(i).gt.0.0) then
           h(i,i) = p(i)/y(i)
          else
           h(i,i)=pp/py
          endif
3101    continue
      end if

      RETURN
      END
C
      SUBROUTINE UNIT(H,N)
      IMPLICIT DOUBLE PRECISION (A-H,O-Z)
      DIMENSION H(N,N)
      COMMON /HSTART/SETB
      DO 1200 I=1,N
        DO 1100 J=1,N
1100      H(I,J)=0.0D0
1200    H(I,I)=SETB
      RETURN
      END
C    
      DOUBLE PRECISION FUNCTION DOTV(N,X,Y)
      IMPLICIT DOUBLE PRECISION(A-H,O-Z)
      DIMENSION X(N),Y(N)
      SUM=0.0
      DO 1100 I=1,N
        SUM=SUM+X(I)*Y(I)
1100  CONTINUE
      DOTV=SUM
      RETURN
      END
C
      DOUBLE PRECISION FUNCTION VTV(M,ME,V,R,G)
      IMPLICIT DOUBLE PRECISION(A-H,O-Z)
C  COMPUTES NORM OF CONSTRAINTS APPEARING IN AUGMENTED LAGRANGIAN
      DIMENSION G(M),V(M),R(M)
      SUM=0.0
      DO 1100 I=1,M
        IF(.NOT.(I.GT.ME.AND.G(I).GT.0.5*R(I)*V(I))) SUM=SUM+G(I)*G(I)
1100  CONTINUE
      VTV=SUM
      RETURN
      END
C
      DOUBLE PRECISION FUNCTION ATA(M,ME,U,G)
C COMPUTES NORM OF ACTIVE CONSTRAINTS
      IMPLICIT DOUBLE PRECISION(A-H,O-Z)
      DIMENSION G(M),U(M)
      SUM=0.0
      DO 1100 I=1,M
        IF(.NOT.(I.GT.ME.AND.U(I).LE.0.0.AND.G(I).GT.0.0))
     1          SUM=SUM+G(I)*G(I)
1100  CONTINUE
      ATA=SUM
      RETURN
      END
C    
      SUBROUTINE OUTPUT(N,M,X,FF,G,U,V,R,IFV,IGV,ISTOP,IOC)
      IMPLICIT DOUBLE PRECISION(A-H,O-Z)
      DIMENSION X(N),G(M),U(M),R(M),V(M)
      WRITE(IOC,2011)
      WRITE(IOC,2021) (X(I),I=1,N)
      WRITE(IOC,2031) FF
C      WRITE(IOC,2041)
C      WRITE(IOC,2021) (G(I),I=1,M)
C      IF(IFV.GT.1) CALL OUTLM(M,U,V,IOC)
C      WRITE(IOC,3111)
C      WRITE(IOC,3121) (R(I),I=1,M)
      WRITE(IOC,3211) IFV,IGV
      IF(ISTOP.EQ.0) RETURN
      IF(ISTOP.EQ.1) WRITE(IOC,4001)
      IF(ISTOP.EQ.3) WRITE(IOC,4003)
      IF(ISTOP.EQ.5) WRITE(IOC,4005)
      IF(ISTOP.EQ.6) WRITE(IOC,4006)
      IF(ISTOP.EQ.14) WRITE(IOC,4014)
      IF(ISTOP.EQ.15) WRITE(IOC,4015)
      RETURN
2011  FORMAT(/' VARIABLES')
2021  FORMAT(1P4D16.7)
2031  FORMAT(' FUNCTION ',1PD16.7)
2041  FORMAT(' CONSTRAINTS')
3111  FORMAT(' PENALTY PARAMETERS')
3121  FORMAT(1P4D15.2)
3211  FORMAT(I5,' FUNCTION CALLS'/,I5,' GRADIENT CALLS')
4001  FORMAT(' CONVERGED'/)
4003  FORMAT(' MORE THAN IMAX ITERATIONS'/)
4005  FORMAT(' NO FURTHER REDUCTION IN FUNCTION OR CONSTRAINTS'/)
4006  FORMAT(' WRONG NUMERICAL DERIVATIVE SUBROUTINE'/)
4014  FORMAT(' LAGRANGE MULTIPLIER EQUATIONS ILL-CONDITIONED'/)
4015  FORMAT(' SEARCH DIRECTION UPHILL'/)
      END
C
      SUBROUTINE OUTLM(M,U,V,IOC)
      IMPLICIT DOUBLE PRECISION(A-H,O-Z)
      DIMENSION U(M),V(M)
      WRITE(IOC,3011)
      WRITE(IOC,3021) (U(I),I=1,M)
      WRITE(IOC,3031)
      WRITE(IOC,3021) (V(I),I=1,M)
      RETURN
3011  FORMAT(' LAGRANGE MULTIPLIERS(U)')
3021  FORMAT(1P4D16.7)
3031  FORMAT(' LAGRANGE MULTIPLIERS(V)')
      END
C 
      SUBROUTINE QPOUT(N,M,X,G,U,ICT,ISTOP,IOC)
C
C PROVIDES OUTPUT FROM IQP SUBROUTINE . NOT NORMALLY USED EXCEPT FOR
C PROGRAM DEVELOPMENT AND DE-BUGGING.
      IMPLICIT DOUBLE PRECISION (A-H,O-Z)
      DIMENSION X(N),G(M),U(M)
      WRITE(IOC,3011)
      WRITE(IOC,3021) (U(I),I=1,M)
      WRITE(IOC,3031)
      WRITE(IOC,3021) (X(I),I=1,N)
      WRITE(IOC,3041)
      WRITE(IOC,3021) (G(I),I=1,M)
      WRITE(IOC,3051) ICT
      IF(ISTOP.EQ.0) RETURN
      IF(ISTOP.EQ.1) WRITE(IOC,4001)
      IF(ISTOP.EQ.3) WRITE(IOC,4003)
      IF(ISTOP.EQ.5) WRITE(IOC,4005)
      IF(ISTOP.EQ.14) WRITE(IOC,4014)
      IF(ISTOP.EQ.15) WRITE(IOC,4015)
      RETURN
3011  FORMAT(' SUBPROBLEM LAGRANGE MULTIPLIERS  ')
3021  FORMAT(1P6D16.7)
3031  FORMAT(' CORRECTION VECTOR')
3041  FORMAT(' LINEARISED CONSTRAINTS')
3051  FORMAT(I5,' QP SUBPROBLEM ITERATIONS')
4001  FORMAT(' QP SUBPROBLEM CONVERGED')
4003  FORMAT(' MORE THAN IMAX ITERATIONS IN QP SUBPROBLEM')
4005  FORMAT(' NO FURTHER PROGRESS IN QP SUBPROBLEM')
4014  FORMAT(' LAGRANGE MULTIPLIER EQUATIONS ILL-CONDITIONED')
4015  FORMAT(' SEARCH DIRECTION UPHILL IN QP SUBPROBLEM')
      END
C
      SUBROUTINE VALUES
      IMPLICIT DOUBLE PRECISION(A-H,O-Z)
      COMMON/HMAT/MATRIX /QPIT/MAXIT
      COMMON/LSCH/ETA1,ETA2 /XI/V1,V2
      COMMON/DAMP/TOL /HSTART/SETB
      COMMON/DEVICE/IOC
      WRITE(IOC,1211)
      WRITE(IOC,1221) V1,V2
      WRITE(IOC,1231) ETA1,ETA2
      WRITE(IOC,1241) MAXIT
      WRITE(IOC,1251) TOL
      IF(MATRIX.EQ.1) WRITE(IOC,1411)
      IF(MATRIX.EQ.0) WRITE(IOC,1421)
      WRITE(IOC,1511) SETB
      WRITE(IOC,1521)
1211  FORMAT(1H1,/,1X,16('*'),' OPALQP.FOR  VERSION 1  ',16('*'))
1221  FORMAT(' GLOBAL CONVERGENCE PARAMETERS  E1,E2=',1P2D10.2)
1231  FORMAT(' LINE SEARCH PARAMETERS     ETA1,ETA2=',1P2D10.2)
1241  FORMAT(' LIMIT ON QP SUBPROBLEM ITERATIONS=',I4)
1251  FORMAT(' PY > ',F4.2,'*SQRT(PBP*YHY) IN MATRIX UPDATE')
1411  FORMAT(' UPDATE BASED ON GRADP')
1421  FORMAT(' UPDATE BASED ON GRADL')
1511  FORMAT(' HESSIAN INITIALISED AS ',1PD10.2,'*I')
1521  FORMAT(1X,56('*'))
      RETURN
      END
C
      BLOCK DATA ALQPPF
      IMPLICIT DOUBLE PRECISION (A-H,O-Z)
      COMMON/LSCH/ETA1,ETA2          /SMALL/STPMIN
      COMMON/DEVICE/IOC /HMAT/MATRIX /HSTART/SETB
      COMMON/DAMP/TOL   /HCOND/COND  /LDSING/XMIN
      COMMON/XI/V1,V2   /QPIT/IMAX   /NDCHK/MAGIC
      DATA IOC,IMAX,MATRIX/6,20,0/
      DATA ETA1,ETA2,COND,XMIN,MAGIC/0.1D0,0.25D0,1.D10,5.D-12,67893/
      DATA TOL,STPMIN,V1,V2,SETB/0.1D0,5.D-12,0.75D0,1.0D0,1.0D0/
      END