C
C
C ********************************************************************
C
C     Cubic Spline with a Constraint
C
C     Optimal Control Problem with a Second Order
C     State Variable Inequality Constraint    
C     due to John V. Breakwell.
C
C     Ref. A.E. Bryson, Y.C. Ho: Applied Optimal Control,
C     Blaisdell Publishing Company, Waltham Ma., 1969. 
C
C     O p t i m a l  C o n t r o l  P r o b l e m:
C
C     Minimize J = 1/2 int_0^1 u^2(t) dt, subject to
C
C     x' = v,     x(0) =  x(1) = 0,
C     v' = u,     v(0) = -v(1) = 1,
C
C     x(t) - x_max <= 0.
C
C     For   x_max >= 1/4       : the solution is a unconstraint 
C                                parabola. 
C     For   1/6 <= x_max < 1/4 : the solution has one contact point.
C     For   0 < x_max < 1/6    : the solution contains one boundary
C                                subarc 
C
C     Boundary Value Problem for a Solution with one
C     Boundary Subarc (obtained by Jacobson's necessary conditions):
C
C     x'        =  v, on [0,t1] and [t2,1]
C                  0, on [t1,t2]
C     v'        = -lambda2, on [0,t1] and [t2,1]
C                  0, on [t1,t2]
C     lambda_1' =  0,
C     lambda_2' = -lambda1, on [0,t1] and [t2,1]
C                  0, on [t1,t2]
C     ell'      =  0,
C
C     x(0)  =  0,     x(1)  =  0,
C     v(0)  =  1,     v(1)  = -1,
C     x(t1) =  x_max, v(t1) =  0,
C     lambda2(t1) = 0 ,
C
C     lambda1(t1+) = 0, lambda1(t2+) = - ell         
C
C ********************************************************************
C
      IMPLICIT DOUBLE PRECISION (A-H,O-Z)
      PARAMETER (MMAX=11,MSMAX=4,MMS=15,NMS=11,
     *           NDW=NMS*(8+MMAX*(10+6*NMS)),NDIW=5*NMS)
      DIMENSION X(MMS),XS(MSMAX),Y(NMS,MMS),WORK(NDW)
      INTEGER JS(MMS,MSMAX),IWORK(NDIW)
      EXTERNAL F, R, DIFSYB, RKF7B
      COMMON /ITEIL/ ITEIL
      COMMON /PARA/ X0, XF, V0, VF, XMAX, U, HAM
C
      OPEN(6,FILE='bryson.txt',STATUS='UNKNOWN')
      OPEN(7,FILE='bryson1.dat',STATUS='UNKNOWN')
      OPEN(8,FILE='bryson2.dat',STATUS='UNKNOWN')
      OPEN(9,FILE='bryson3.dat',STATUS='UNKNOWN')
C
      NFILE =   6
C
C --- Parameters
C
      X0     =   0.D0
      XF     =   0.D0
      V0     =   1.D0
      VF     =  -1.D0
C
      N      =  4
      M      =  6
      MS     =  0
      TOL    =  1.D-07
C
C --- Starting trajectory
C
      DO 100 K=1,M
      X(K)   =  FLOAT(K-1)/FLOAT(M-1)
      Y(1,K) =  0.D0
      Y(2,K) =  1.D0
      Y(3,K) =  0.D0
      Y(4,K) =  0.D0
 100  CONTINUE
C
C  -- A. Unconstrained Spline
C
      ITEIL  =  1
      WRITE(NFILE,1001)
      KP     =  0
      KS     =  1
      ITMAX  =  20
      CALL BNDSCO(F,R,DIFSYB,X,XS,Y,WORK,IWORK,JS,N,M,MS,
     *     KS,TOL,ITMAX,KP,MMAX,MSMAX,MMS,NMS,NDW,NDIW,NFILE)
      IF(KP.LT.0) GOTO 900
C
      ITEIL  =  2
C
      NPLOT  =  7 
      KP     =  0
      IFEIN  =  5
      IPLOT  =  1
      ICASE  =  1
      CALL AWP(DIFSYB,F,X,XS,Y,N,M,MS,
     *         JS,MMS,NMS,IFEIN,IPLOT,KP,NFILE,NPLOT,ICASE)
C
C  -- B. Spline with one Touching Point
C
      ITEIL  =  3
      WRITE(NFILE,1002)
C
      XMAX   =  0.2D0
C
      KP     =  0
      KS     =  1
      ITMAX  =  20
C
      N      =  5
      MS     =  1
      DO 200 K=1,M
      Y(5,K) =  0.D0
 200  CONTINUE
      XS(1)  =  0.3D0
      CALL BNDSCO(F,R,DIFSYB,X,XS,Y,WORK,IWORK,JS,N,M,MS,
     *     KS,TOL,ITMAX,KP,MMAX,MSMAX,MMS,NMS,NDW,NDIW,NFILE)
      IF(KP.LT.0) GOTO 900
C
      ITEIL  =  4
C
      NPLOT  =  8 
      KP     =  0
      IFEIN  =  5
      IPLOT  =  1
      ICASE  =  1
      CALL AWP(DIFSYB,F,X,XS,Y,N,M,MS,
     *         JS,MMS,NMS,IFEIN,IPLOT,KP,NFILE,NPLOT,ICASE)
C
C  -- C. Spline with a Boundary Subarc
C
      ITEIL  =  5
      WRITE(NFILE,1003)
C
      CALL RESET(X,XS,Y,N,M,MS,NMS)
      KP     =  0
      KS     =  1
      ITMAX  =  20
      XMAX   =  0.1D0
      N      =  5
      MS     =  2
      XS(1)  =  0.4D0
      XS(2)  =  0.6D0 
      CALL BNDSCO(F,R,DIFSYB,X,XS,Y,WORK,IWORK,JS,N,M,MS,
     *     KS,TOL,ITMAX,KP,MMAX,MSMAX,MMS,NMS,NDW,NDIW,NFILE)
      IF(KP.LT.0) GOTO 900
C
      ITEIL  =  6
C
      NPLOT  =  9 
      KP     =  0
      IFEIN  =  5
      IPLOT  =  1
      ICASE  =  1
      CALL AWP(DIFSYB,F,X,XS,Y,N,M,MS,
     *         JS,MMS,NMS,IFEIN,IPLOT,KP,NFILE,NPLOT,ICASE)
C
  900 CONTINUE
 1001 FORMAT(//15X,'A. Unconstrained Problem:')
 1002 FORMAT(//15X,'B. One Contact Point:')
 1003 FORMAT(//15X,'C. One Boundary Subarc:')
C -------------------------------------------------------------------
      STOP
      END




      SUBROUTINE F(X,Y,DY,J,L,JS,MMS)
C
C     Cubic Spline with a State Constraint
C
      IMPLICIT DOUBLE PRECISION (A-H,O-Z)
      DIMENSION Y(*),DY(*)
      INTEGER JS(MMS,*)
      COMMON /ITEIL/ ITEIL
      COMMON /PARA/ X0, XF, V0, VF, XMAX, U, HAM
C
      U     = -Y(4)
C
      DY(1) =  Y(2)
      DY(2) =  U
      DY(3) =  0.D0
      DY(4) = -Y(3)
C
      HAM   =  0.5D0*U*U + Y(3)*Y(2) + Y(4)*U
C
      IF(ITEIL.EQ.1) RETURN
      IF(ITEIL.EQ.2) THEN
      DY(5) = 0.5D0*U*U
      RETURN
      ENDIF
C
      DY(5) =  0.D0
      IF(ITEIL.EQ.3) RETURN
      IF(ITEIL.EQ.4) THEN
      DY(6) = 0.5D0*U*U
      RETURN
      ENDIF
C
      IF(JS(J,1)*JS(J,2).LT.0) THEN
        DY(1) =  0.D0
        DY(2) =  0.D0
        DY(4) =  0.D0
        U     =  0.D0 
      ENDIF 
C
      IF(ITEIL.EQ.6) DY(6) = 0.5D0*U*U
C -------------------------------------------------------------------
      RETURN
      END




      SUBROUTINE R(YA,YB,ZZ,W,NYA,NSK,J,L,LS,JS,MMS)
C
C     Cubic Spline with a State Constraint
C
      IMPLICIT DOUBLE PRECISION (A-H,O-Z)
      DIMENSION YA(*),YB(*),ZZ(*),W(*)
      INTEGER NYA(*),NSK(*)
      INTEGER JS(MMS,*)
      COMMON /ITEIL/ ITEIL
      COMMON /PARA/ X0, XF, V0, VF, XMAX, U, HAM
C
      W(1)   =  YA(1) - X0
      W(2)   =  YA(2) - V0
      W(3)   =  YB(1) - XF
      W(4)   =  YB(2) - VF
C
      NYA(1) =  1
      NYA(2) =  2
C
      IF(ITEIL.EQ.1) RETURN
C
      IF(ITEIL.EQ.3) THEN
C
      W(5)   =  ZZ(1) - XMAX
      W(6)   =  ZZ(2)
      NSK(5) =  1
      NSK(6) =  1
      IF(LS.EQ.1) ZZ(3) = ZZ(3) - ZZ(5)
      RETURN
      ENDIF
C
      IF(ITEIL.EQ.5) THEN
      W(5)   =  ZZ(1) - XMAX
      W(6)   =  ZZ(2)
      W(7)   =  ZZ(4)
      NSK(7) =  1
      NSK(5) =  1
      NSK(6) =  1
      IF(LS.EQ.1) ZZ(3) = 0.D0
      IF(LS.EQ.2) ZZ(3) = -ZZ(5)
      ENDIF
C
C -------------------------------------------------------------------
      RETURN
      END




      SUBROUTINE AWP(METHOD,FCN,X,XS,Y,N,M,MS,JS,MMS,NMS,IFEIN,
     *               IPLOT,KP,NFILE,NPLOT,ICASE)
C
C *******************************************************************
C
C     INTEGRATION OF INITIAL VALUE PROBLEMS
C
C     IFEIN : NUMBER OF INTEGRATION NODES IN SUBINTERVAL
C     IPLOT : THE NODES ARE STORED FOR PLOT SUBROUTINE,
C             IF IPLOT.EQ.1 THE ROUTINE TAKES IFEIN = 40
C             EQUIDISTANT POINTS IN EACH SUBINTERVAL
C     KP    : PRINT PARAMETER
C             KP.LT.0 : NO PRINT
C             KP.EQ.0 : MULTIPLE SHOOTING NODES
C             KP.GT.0 : ALL INTEGRATION NODES
C     ICASE : 1  :  ROUTINE INTEGRATES OUTPUT OF BNDSCO IN EACH
C                   SUBINTERVAL (THE SWITCHING POINTS ARE ALREADY
C                   ARRANGED IN ORDER; THE ROUTINE TAKES NEW INITIAL
C                   DATA FOR EACH SUBINTERVAL)
C             0  :  INTEGRATION OVER THE WHOLE INTERVAL; THE ALGORITHM
C                   ARRANGES THE SWITCHING POINTS XS(K) IN ORDER AND
C                   SAVES THE DATA Y(I;J);
C
C *******************************************************************
C
      IMPLICIT DOUBLE PRECISION (A-H,O-Z)
      DIMENSION X(*),XS(*),Y(NMS,*)
      DIMENSION Z(20),DZ(20)
      INTEGER JS(MMS,*)
      EXTERNAL FCN , R
      COMMON /ITEIL/ ITEIL
      COMMON /PARA/ X0, XF, V0, VF, XMAX, U, HAM
C
      IF(IPLOT.GT.0) IFEIN=40
      TOL=1.D-07
C
      NN  =  N + 1
      Z(NN) = 0.D0
C
C     ARRANGING THE SWITCHING POINTS (IN CASE ICASE = 0)
C
      IF(MS.EQ.0.OR.ICASE.GT.0) GOTO 300
      K   =  1
  200 CONTINUE
      K1  =  K + 1
      X1  =  X(K)
      X2  =  X(K1)
      DO 210 L=1,MS
      IF((X1-XS(L))*(X2-XS(L)).LT.-1.D-10) GOTO 230
  210 CONTINUE
      GOTO 250
  230 CONTINUE
      M   =  M + 1
      DO 240 J=M,K+2,-1
      X(J) = X(J-1)
  240 CONTINUE
      X(K1) = XS(L)
      GOTO 200
  250 K   =  K1
      IF(K.LT.M) GOTO 200
  300 CONTINUE
C
      JT  =  0
      L   =  0
      HI  =  0.D0
      M1  =  M - 1
      IF(KP.GE.0) THEN
        WRITE(NFILE,2000)
        WRITE(NFILE,2200)
      ENDIF
C
C     LOOP OVER THE SUBINTERVALS
C
      DO 500 K=1,M1
      J=K
      J1=J+1
      L=L+1
C
      T=X(J)
      TEND=X(J1)
      IF(MS.GT.0) THEN
         DO 503 KL=1,MS
  503    IF(T.EQ.XS(KL)) L=L-1
      ENDIF
      HMAX=(TEND-T)/IFEIN
      IF(HI.EQ.0.D0) HI=0.01D0*HMAX
      HMAX=ABS(HMAX)
C
C     DEFINITION OF JS(J,K), K=1,..,MS (ICASE = 0)
C
      IF(ICASE.EQ.0) THEN
      DO 410 KL=1,MS
      JS(J,KL) = 1
      IF(T.LT.XS(KL)-1.D-10) JS(J,KL) = -1
  410 CONTINUE
      END IF
C
C     NEW INITIAL DATA FOR THE INTEGRATION
C
      IF(ICASE.EQ.0.AND.J.GT.1) GOTO 430
      DO 420 I=1,N
  420 Z(I)=Y(I,J)
  430 CONTINUE
C
      CALL FCN(T,Z,DZ,J,L,JS,MMS)
C
      IF(KP.GE.0) WRITE(NFILE,2001)
      JT=JT+1
      IF(KP.GE.0)
     *WRITE(NFILE,3000) T,(Z(I),I=1,4), Z(NN), U, HAM
      IF(IPLOT.EQ.1)
     *WRITE(NPLOT,4000) T,(Z(I),I=1,4), Z(NN), U, HAM
C
C     LOOP FOR SUB-SUB-INTERVALS
C
      DO 510 K1=1,IFEIN
      T1=T+SIGN(HMAX,HI)
      IF(K1.EQ.IFEIN) T1=TEND
C
      CALL METHOD(NN,FCN,T,Z,T1,TOL,HMAX,HI,J,L,JS,MMS)
C
      IF(HI.EQ.0.D0) GOTO 600
C
      CALL FCN(T,Z,DZ,J,L,JS,MMS)
C
      JT=JT+1
      IF(KP.GT.0)
     *WRITE(NFILE,3000) T,(Z(I),I=1,4), Z(NN), U, HAM
      IF(IPLOT.EQ.1)
     *WRITE(NPLOT,4000) T,(Z(I),I=1,4), Z(NN), U, HAM
C
  510 CONTINUE
C
C     DEFINITION OF THE MULTIPLE SHOOTING KNOTS (ICASE = 0)
C
      IF(ICASE.EQ.0) THEN
      DO 520 I=1,NN
      Y(I,J+1) = Z(I)
  520 CONTINUE
      ENDIF
C
  500 CONTINUE
C
      IF(KP.EQ.0) THEN
      WRITE(NFILE,2001)
      WRITE(NFILE,3000) T,(Z(I),I=1,4), Z(NN),U, HAM
      ENDIF
      IF(KP.GE.0) WRITE(NFILE,2001)
      GOTO 900
C
  600 CONTINUE
      WRITE(NFILE,5000)
      WRITE(NFILE,3000) T,(Z(I),I=1,4),Z(NN),U, HAM
C
  900 CONTINUE
 1000 FORMAT(I4)
 2000 FORMAT(///)
 2001 FORMAT(' ',77('*'))
 2200 FORMAT(7X,'t',17X,'x',17X,'v',17X,'Lx'/
     *      ,25X,'Lv',16X,'Ziel',14X,'u'/
     *      ,25X,'Ham')
 3000 FORMAT(4D18.10/5(18X,3D18.10/))
 4000 FORMAT(5(30E13.5/))
 5000 FORMAT(' INTEGRATIONSABBRUCH: SCHRITTWEITE H = 0')
C -------------------------------------------------------------------
      RETURN
      END



      SUBROUTINE RESET(X,XS,Y,N,M,MS,NMS)
C
      IMPLICIT DOUBLE PRECISION (A-H,O-Z)
      DIMENSION X(*),XS(*),Y(NMS,*)
C
      J1=0
      MD=M
      DO 600 J2=1,M
      DO 601 I=1,MS
      IF(XS(I).EQ.X(J2)) GOTO 620
  601 CONTINUE
      J1=J1+1
      X(J1)=X(J2)
      DO 610 I=1,N
  610 Y(I,J1)=Y(I,J2)
      GOTO 600
  620 MD=MD-1
  600 CONTINUE
      M=MD
C -------------------------------------------------------------------
      RETURN
      END


