C
C
C ********************************************************************
C
C     Problem of Goddard:
C
C     Maximize the final altitude for the vertical ascent
C     of a single-stage rocket with a fixed amount of fuel
C
C     Here: Gavitational force g constant, 
C           Scaling of mass m(0) = 1.
C           Solution with a Singular Subarc
C
C     Minimize I = -h(tf)
C     subject to
C     h'  =  v              , h(0) = 0 , 
C     v'  =  (cu - D)/m - g , v(0) = 0 ,  v(tf) = 0,
C     m'  = -u              , m(0) = 1 ,  m(tf) = mf,
C     0 <= u(t) <= u_max
C     D := D0 * exp(-alpha*h)*v^2
C    
C     Control Structure: u: u_max - u_sing - 0
C
C     Ref.:
C
C     R.H. Goddard : A Method for Reachimg Extreme Altitudes.
C                    Smithsonian Collection 2, 1919.
C     G. Hamel     : Ueber eine mit dem Problem der Rakete
C                    zusammenhaengende Aufgabe. ZAMM 7, 451-552, 1927.        
C     A. Miele     : Generalized Variational Approach to the Optimum
C                    Thrust Programming for a Vertical Flight  of a
C                    Rocket. ZFW VOL.6 No.3 and No.4, 1958.
C     H.J.Diekhoff : Ueber die optimale Steuerung einer Hoehenrakete.
C                    Diplomarbeit. Universitaet zu Koeln, 1973.
C     M. Maurer    : Numerical Solution of Singular Control Problems
C                    Using Multiple Shooting Techniques.
C                    J. of Optim. Theory a. Appl. Vol.18, No.2, 1976.
C
C ********************************************************************
C
C
      IMPLICIT DOUBLE PRECISION (A-H,O-Z)
      PARAMETER (MMAX=11,MSMAX=2,MMS=13,NMS=12,
     *           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
      COMMON /PARA/ XM0,XMF,D0,ALPHA,CB,G0,UMAX,
     *              TF,U,SWIT,SWIT1,HAM
      COMMON /ITEIL/ ITEIL
C
      OPEN(6,FILE='goddard.txt',STATUS='UNKNOWN')
      OPEN(7,FILE='goddard.dat',STATUS='UNKNOWN')
      OPEN(8,FILE='godd01.dat',STATUS='UNKNOWN')
      OPEN(9,FILE='godd02.dat',STATUS='UNKNOWN')
C
C --- Parameters
C
      XM0    =  215.D0
      XMF    =  68.D0/XM0
      CD     =  0.280D0
      FD     =  0.570D-7
      RHO0   =  0.154D10
      D0     =  0.5D0*RHO0*FD*CD/XM0 
      ALPHA  =  0.145D0
      CB     =  2.06D0
      G0     =  0.981D-2
      UMAX   =  9.5D0/XM0
C
      N      =  7
      M      =  6
      MS     =  2
      TOL    =  1.D-8
      KS     =  1
      NFILE  =  6
      WRITE(6,1000)
C
C --- Initial Trajectory
C
      DO 100 J=1,M
      READ(8,1010) X(J),(Y(I,J),I=1,N)
  100 CONTINUE
      READ(8,1010) (XS(K),K=1,MS) 
C
      ITEIL  =  1
      KP     =  0
      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
      KP     =  0
      IFEIN  =  5
      NPLOT  =  7
      IPLOT  =  1
      ICASE  =  1
      CALL AWP(DIFSYB,F,X,XS,Y,N,M,MS,
     *         JS,MMS,NMS,IFEIN,IPLOT,KP,NFILE,NPLOT,ICASE)
C
      CALL RESET(X,XS,Y,N,M,MS,NMS)
      DO 800 J=1,M
      WRITE(9,1010) X(J),(Y(I,J),I=1,N)
  800 CONTINUE
      WRITE(9,1010) (XS(K),K=1,MS)
C
  900 CONTINUE
C
 1000 FORMAT(//' Problem of Goddard: Solution with a Singular Subarc')
 1010 FORMAT(4D20.12)
C -------------------------------------------------------------------
      STOP
      END



      SUBROUTINE F(X,Y,DY,J,L,JS,MMS)
C
      IMPLICIT DOUBLE PRECISION (A-H,O-Z)
      DIMENSION Y(*),DY(*)
      INTEGER JS(MMS,*)
      COMMON /PARA/ XM0,XMF,D0,ALPHA,CB,G0,UMAX,
     *              TF,U,SWIT,SWIT1,HAM
      COMMON /ITEIL/ ITEIL
C
      H     =  Y(1)
      V     =  Y(2)
      XM    =  Y(3)
      TF    =  Y(4)
      HL    =  Y(5)
      VL    =  Y(6)
      XML   =  Y(7)
C
      D     =  D0*DEXP(-ALPHA*H)*V*V
C
      DH    = -ALPHA*D 
      DV    =  2.D0*D0*DEXP(-ALPHA*H)*V
      DVH   = -ALPHA * DV
      DVV   =  2.D0*D0*DEXP(-ALPHA*H)
C
      U     = 0.D0
      IF(JS(J,1).LT.0) U = UMAX
      IF(JS(J,1)*JS(J,2).GT.0) GOTO 100
      XNEN  =  D + 2.D0*CB*DV + CB*CB*DVV
      XZAE  =  (CB-V)*DH + G0*DV + CB*G0*DVV - CB*V*DVH 
      U     =  D/CB + XM*XZAE/XNEN
C
  100 CONTINUE
      DY(1) =  TF*V
      DY(2) =  TF*((CB*U-D)/XM - G0)
      DY(3) = -TF*U
      DY(4) =  0.D0
C
      DY(5) =  TF*VL*DH/XM 
      DY(6) =  TF*(-HL + VL*DV/XM)
      DY(7) =  TF*VL*(CB*U - D)/XM/XM
C
      SWIT  =  CB*VL/XM -XML
      SWIT1 = -CB*HL/XM + VL*(D+CB*DV)/XM/XM
      HAM   =  (HL*DY(1)+VL*DY(2)+XML*DY(3))/TF 
C -------------------------------------------------------------------
      RETURN
      END



      SUBROUTINE R(YA,YB,ZZ,W,NYA,NSK,J,L,LS,JS,MMS)
C
      IMPLICIT DOUBLE PRECISION (A-H,O-Z)
      DIMENSION YA(*),YB(*),ZZ(*),W(*)
      INTEGER NYA(*),NSK(*)
      INTEGER JS(MMS,*)
      COMMON /PARA/ XM0,XMF,D0,ALPHA,CB,G0,UMAX,
     *              TF,U,SWIT,SWIT1,HAM
      COMMON /ITEIL/ ITEIL
C
      W(1) =  YA(1)
      W(2) =  YA(2)
      W(3) =  YA(3) - 1.D0
      W(4) =  YB(2)
      W(5) =  YB(3)/XMF - 1.D0
C
      NYA(1) = 1
      NYA(2) = 2
      NYA(3) = 3
C
      W(6) =  YB(5) + 1.D0
      W(7) =  SWIT
      W(8) =  SWIT
C
      NSK(7) = 1
      NSK(8) = 2  
C
      W(9)   = HAM  
      NSK(9) = 1
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 /PARA/ XM0,XMF,D0,ALPHA,CB,G0,UMAX,
     *              TF,U,SWIT,SWIT1,HAM 
      COMMON /ITEIL/ ITEIL
C
      IF(IPLOT.GT.0) IFEIN=40
      TOL=1.D-07
C
      NN  =  N
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)
        WRITE(NFILE,2001)
      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.GT.0) WRITE(NFILE,2001)
      JT=JT+1
      IF(KP.GE.0)
     *WRITE(NFILE,3000) T,(Z(I),I=1,NN), U, SWIT, SWIT1, HAM
      IF(IPLOT.EQ.1)
     *WRITE(NPLOT,4000) T,(Z(I),I=1,NN), U, SWIT, SWIT1, 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,NN), U, SWIT, SWIT1, HAM
      IF(IPLOT.EQ.1)
     *WRITE(NPLOT,4000) T,(Z(I),I=1,NN), U, SWIT, SWIT1, 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,3000) T,(Z(I),I=1,NN), U, SWIT, SWIT1, 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,NN), U, SWIT, SWIT1, HAM
C
  900 CONTINUE
 1000 FORMAT(I4)
 2000 FORMAT(///)
 2001 FORMAT(' ',77('*'))
 2200 FORMAT(7X,'t',17X,'H',17X,'V',17X,'M'/
     *       25X,'TF',16X,'HL',16X,'VL'/
     *       25X,'ML',16X,'U',17X,'SWIT'/
     *       25X,'SWIT1',13X,'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



