C     PROGRAM KOZTST
C      
C     THIS IS A MAIN PROGRAM FOR USING SUBROUTINE KOZSAK, A
C     FORTRAN SUBROUTINE TO CONVERT FROM MEAN TO OSCULATING 
C     ORBITAL ELEMENTS AND VICE-VERSA. THIS MAIN PROGRAM ALSO 
C     PROPAGATES THE MEAN STATE FORWARD IN TIME TO PROVIDE AN 
C     EPHEMERIS AND A TIME HISTORY OF THE MEAN AND OSCULATING
C     ORBITAL ELEMENTS AND,IF REQUESTED,THE OSCULATING POSITION
C     AND VELOCITY. THIS WHOLE PACKAGE IS DESIGNED TO BE AS 
C     MACHINE INDEPENDENT AS POSSIBLE; PLEASE EXCUSE THE 
C     NON-MACINTOSH INTERFACE WITH THE USER.
C          INPUTS ARE AS FOLLOWS:
C     
C     OUTFIL .... CHARACTER*64 FILENAME PATH FOR UNIT 6 OUTPUT
C     OUTFLX .... CHARACTER*64 FILENAME PATH FOR UNIT 99 OUTPUT
C                 THIS IS TAB-DELIMITED OUTPUT FOR PASTING INTO
C                 PLOTTING PACKAGES LIKE CRICKET GRAPH OR EXCEL.
C     
C     A,E,F,W,AI,OM .... INPUT ORBITAL ELEMENTS
C           IN TRUE EQUATOR AND EQUINOX OF EPOCH
C
C           A ..SEMI-MAJOR AXIS(KM)  ,   E .. ECCENTRICITY
C           F .. TRUE ANOMALY(DEG) , W .. ARG. OF PERICENTER(DEG)
C           AI . INCLINATION (DEG) , OM . LONG. OF ASC. NODE (DEG)
C
C   
C           NOTE:  THESE MAY BE EITHER OSCULATING OR MEAN ELEMENTS
C                  DEPENDING ON THE SETTING OF THE FLAG MNOSC.
C
C     RUN CONTROL PARAMETERS
C
C     MNOSC IS THE FLAG TO SPECIFY WHETHER THE INPUT ELEMENTS ABOVE
C           ARE MEAN OR OSCULATING. IF MNOSC IS POSITIVE ( OR ZERO) 
C           THEN WE ARE GOING FROM MEAN TO OSCULATING AND THE PROGRAM 
C           ASSUMES YOU INPUT MEAN ELEMENTS.  IF MNOSC IS 1, PROGRAM ASSUMES
C           SINGLY-AVERAGED MEAN ELEMENTS ARE INPUT. IF MNOSC =0, PROGRAM 
C           ASSUMES AKSNES THEORY MEAN ELEMENTS HAVE BEEN INPUT.    
C	    IF MNOSC IS NEGATIVE, THE  INPUT ELEMENTS WILL BE TREATED AS
C	    OSCULATING.  AN ITERATION WILL BE PERFORMED ABS(MNOSC) TIMES
C	    TO GO FROM OSCULATING TO MEAN INITIAL ELEMENTS. SET TO -3 OR -4
C	    FOR CONSISTENT RESULTS.
C           SEE SUBROUTINE KOZSAK FOR USE OF MNOSC IN ITERATION.
C
C     KPROP IS THE FLAG TO PROPAGATE MEAN ELEMENTS FORWARD AND GENERATE AN
C           EPHEMERIS.  IF KPROP = 0, THEN CONVERSION WILL BE DONE ONLY FOR 
C           INITIAL INPUT ELEMENTS.
C
C     KOUT  IS OUTPUT FLAG FOR EPHEMERIS. IF KOUT = 0  .. NO OUTPUT
C                                            KOUT < 0  .. UNIT 6 OUTPUT
C                                            KOUT > 0  .. UNIT 6 AND SCREEN
C
C     KU99 IS OUTPUT FLAG FOR TAB-DELIMITED (UNIT 99) OUTPUT
C             IF KU99 = 0 ..  NO UNIT 99 OUTPUT
C	        KU99 = 1 .. OSCULATING ELEMENTS (KM , DEG)
C		KU99 = 2 .. OSCULATING POSITION AND VELOCITY (KM,KM/S)
C		KU99 = 3 .. BOTH 1 AND 2 (ON ONE LINE)
C               KU99 = 4 .. J2-ONLY MEAN ELEMENTS (KM, DEG)
C               KU99 = 5 .. 1 AND 4 ON ONE LINE
C
C     DELT IS THE TIME INCREMENT FOR THE EPHEMERIS (SECONDS)
C
C     TF IS THE FINAL TIME FOR THE EPHEMERIS (SECONDS)
C
C     TS IS THE STARTING TIME FOR THE EPHEMERIS (SECONDS RELATIVE TO 
C        THE EPOCH OF THE INPUT ORBITAL ELEMENTS .. USUALLY ZERO.)
C
C     TCON IS TIME CONVERSION FACTOR FOR PRINTOUT.  
C          OUTPUT WILL BE PRINTED AS T*TCON.
C
C     NODALP IS A FLAG TO CALCULATE THE NODAL PERIOD OF THE INPUT ORBIT.
C
C     NREV IS THE NUMBER OF REVS FOR WHICH NODAL PERIOD IS DESIRED.
C
C     J2SQ IS FLAG TO INCLUDE J2 SQUARED TERMS IN MEAN ELEMENTS PROPAGATION.
C
C     THIS CODE WAS DEVELOPED ON THE MACINTOSH PLUS, SE, AND II USING 
C     THE ABSOFT MACFORTRAN/020 VERSION 2.3.  REFERENCES TO LONG(362)
C     ARE TO GET TO THE SYSTEM CLOCK.  CHANGE FOR PC'S OR OTHER MACHINES.
C     THIS SHOULD BE THE ONLY CHANGE REQUIRED UNLESS YOUR COMPILER WANTS 
C     TO USE UNIT 9 (CODED AS * HERE) TO REFER TO THE SCREEN OR CONSOLE.
C
C     GOOD LUCK ... IF YOU HAVE TROUBLE .. CALL CHAUNCEY UPHOFF X6480
C
C     NOTE:  J2 ,J3, J4 IN 1988 DEC 22 VERSION
C
C                                      1988 DEC 22
C
C
      IMPLICIT REAL*8(A-H,O-Z)
      CHARACTER*64 OUTFIL, OUTFLX
      CHARACTER*4 Q
      DIMENSION ELMOUT(6),ELMIN(6),ELMT(6),ELMTOT(6)
     1  , XOUT(3),XDOUT(3),ELMDUM(6),ELMJ2(6),ELMJ2OT(6)
C
      COMMON /CONST/ RAD,PI,PI2,AU
C     RAD = DEG/RADIAN, PI,PI2 = 2*PI, AU = EARTH TO SUN (KM)
C
      COMMON /GRVMOD/GM,REQ,SPIN,AJ2,AJ3,AJ4,AJ5
C     GM = GRAV CONST OF CENTRAL PLANET ( KM**3/S**2)
C     REQ = EQ. RADIUS OF CENTRAL PLANET (KM)
C     SPIN = ROTATION RATE OF CENTRAL PLANET (RAD/SEC)
C     AJ2,AJ3,AJ4,AJ5 = ZONAL HARMONICS OF CENTRAL PLANET
C     SEE BLOCK DATA SUBROUTINE FOR VALUES LOADED.      
C
C     SETUP AND INPUT
C
      Q = CHAR(09)
      WRITE(*,1010)
      READ (*,*) OUTFIL
      WRITE(*,1030)
      READ(*,*) OUTFLX
      OPEN (6,FILE=OUTFIL)
      OPEN (99,FILE=OUTFLX)
 1010 FORMAT ('Please input the FILENAME path for UNIT 6 output:')
 1030 FORMAT ('Please input the FILENAME path for UNIT 99 output:')
    1 WRITE(*,*) 'Enter Elements (a,e,f,w,i,Node) of Spacecraft:'
      READ(*,*) A,E,F,W,AI,OM
      WRITE(6,444) A,E,F,W,AI,OM
  444 FORMAT('INPUT ORBITAL ELEMENTS:(a,e,f,w,i,Node)'
     1       / F11.4,F10.7,4F10.4)
C     WRITE(99,*)'INITIAL S/C ORBIT: ',A,Q,E,Q,F,Q,W,Q,AI,Q,OM
C
      IF (A .LT. 0.D0) GO TO 999
C
C     LOAD ELEMENTS INTO ELMIN
      ELMIN(1) = A
      ELMIN(2) = E
      ELMIN(3) = F/RAD
      ELMIN(4) = W/RAD
      ELMIN(5) = AI/RAD
      ELMIN(6) = OM/RAD
      WRITE (*,*) 'Please enter run control values:'
      WRITE(*,*)'MNOSC,KPROP,KOUT,KU99,DELT,TF,TS,TCON,NODALP,NREV,J2SQ'
      READ(*,*) MNOSC,KPROP,KOUT,KU99,DELT,TF,TS,TCON,NODALP,NREV,J2SQ
      WRITE(6,*)'MNOSC,KPROP,KOUT,KU99,DELT,TF,TS,TCON,NODALP,NREV,J2SQ'
      WRITE(6,*)MNOSC,KPROP,KOUT,KU99,DELT,TF,TS,TCON,NODALP,NREV,J2SQ
      KSS = LONG(362) !SECRET CODE FOR GETTING TIME TO 1/60 SEC
C     DO 3 I=3,6
C   3 ELMIN(I)=ELMIN(I)/RAD
C
      IF (KPROP.NE.0) GO TO 4
C     
C     ELEMENTS NOT PROPAGATED, CONVERSION OF INPUT ELEMENTS ONLY
C
      CALL KOZSAK(ELMOUT,EM,ELMIN,MNOSC)
      DO 2 I=3,6
      ELMOUT(I) = ELMOUT(I)*RAD
    2 CONTINUE
      EM=EM*RAD
      WRITE(6,111) ELMOUT,EM
      WRITE(*,111) ELMOUT,EM
  111 FORMAT(1H ,F11.4,F10.7,5F10.4)
C
C     MEAN ELEMENTS PROPAGATED
C
    4 IF (KPROP.EQ.0)GO TO 1
      IF (MNOSC.LT.0)GO TO 5
      DO 6 I=1,6
    6 ELMOUT(I) = ELMIN(I)
      IF (MNOSC .NE.1) GO TO 7

    5 IF (MNOSC.NE.1) GO TO 15
C     
C     GET OSC. ELEMENTS FROM J2-ONLY MEAN ELEMENTS
C
      CALL KOZJ2(ELMIN,EM,ELMOUT,MNOSC)
C     
C     GET AKSNES MEAN ELEMENTS FROM OSC. ELEMENTS
C
      CALL KOZSAK(ELMOUT,EM,ELMIN,-3)
      GO TO 7
   15 CALL KOZSAK(ELMOUT,EM,ELMIN,MNOSC)
    7 PBAR= ELMOUT(1)*(1.D0-ELMOUT(2)**2)
      CON = AJ2*REQ*REQ/PBAR/PBAR
      SI= DSIN(ELMOUT(5))
      CI = DCOS(ELMOUT(5))
      SEPS= DSQRT(1.D0-ELMOUT(2)**2)
      FACT = (1.D0-1.5D0*CON*(1.D0-1.5D0*SI*SI)*SEPS)
      ENBAR= DSQRT(GM/ELMOUT(1)**3)
      ENBAR= ENBAR*(1.D0-0.75*CON*SEPS*(1.D0-3.D0*CI*CI))
C
C     NOTE THAT ENBAR IS KOZAI'S MEAN MEAN NOTION BUT THE
C     SEMI-MAJOR AXIS, ELMT(1), IS NOT MULTIPLIED BY FACT AS
C     IN KOZAI'S 1959 PAPER. THE MEAN A HERE IS KOZAI'S ABAR
C      WHICH EQUALS BROUWER'S A''(TO ORDER J2**2)..
C
C     NOTE:   ELMOUT NOW CONTAINS THE INITIAL MEAN ELEMENTS
C
      ELMT(1)=ELMOUT(1)
      ELMT(2)=ELMOUT(2)
      ELMT(5) = ELMOUT(5)
      OM0 = ELMOUT(6)
      W0 = ELMOUT(4)
C     
C     GET INITIAL MEAN MEAN ANOMALY
      EM0 = TRMN(1,ELMOUT(3),ELMT(2))
      T = TS
      GAM4 = AJ4/AJ2**2
      IF(J2SQ.NE.0) OD2 = ENBAR*(3.D0/32.D0)*CON*CON*CI
     1          *(4.D0-40.D0*CI*CI-(9.D0-5.D0*CI*CI)*ELMT(2)**2
     2             + 12.D0*SEPS*(1.D0-3.D0*CI*CI)  
     3 - 5.D0*GAM4*(3.D0 - 7.D0*CI*CI)*(2.D0 + 3.D0*ELMT(2)**2))
      IF(J2SQ.NE.0) WD2 = -ENBAR*0.75D0*CON
     1       *CON/32.D0*((10.D0+86.D0*CI*CI)*(1.D0-5.D0*CI*CI)
     2    +(25.D0-126.D0*CI*CI+45.D0*CI**4)*ELMT(2)**2
     3    -24.D0*SEPS*(1.D0-8.D0*CI*CI+15.D0*CI**4)
     4   +20.D0*GAM4*(3.D0-36.D0*CI*CI+49.D0*CI**4)
     5   +45.D0*GAM4*(1.D0-14.D0*CI*CI+21.D0*CI**4)*ELMT(2)**2)
      IF(J2SQ.NE.0) ENBAR=ENBAR+0.75D0*CON*CON*SEPS/32.D0
     1   *((10.D0-60.D0*CI*CI+130.D0*CI**4)
     2   -(25.D0-90.D0*CI*CI+25.D0*CI**4)*ELMT(2)**2
     3   +16.D0*SEPS*(1.D0-6.D0*CI*CI+9.D0*CI**4)
     4   -15.D0*GAM4*(3.D0-30.D0*CI*CI+35.D0*CI**4)*ELMT(2)**2)*ENBAR
C  
C     TOP OF LOOP FOR EPHEMERIS GENERATION
C
    8 ELMT(6) = OM0 - 1.5D0*CON*CI*ENBAR*T
      ELMT(4) = W0 + 1.5D0*CON*ENBAR*(2.D0-2.5D0*SI*SI)*T
      IF(J2SQ.NE.0) ELMT(4) =  ELMT(4) + WD2*T
      IF(J2SQ.NE.0) ELMT(6) = ELMT(6)  + OD2*T
      ELMT(4) = DMOD(ELMT(4),PI2)
      ELMT(6) = DMOD(ELMT(6),PI2)      
      EMT= EM0 + ENBAR*T
      EMT = DMOD(EMT,PI2)
      ELMT(3) = TRMN(-1,EMT,ELMT(2))
      ELMTOT(1) = ELMT(1)
      ELMTOT(2) = ELMT(2)
      DO 14 I=3,6
   14 ELMTOT(I) = ELMT(I)*RAD
C     
C     MEAN ELEMENTS NOW UPDATED
C
C     NOW GET OSCULATING ELEMENTS AT TIME T
C
      CALL KOZSAK(ELMDUM,EM,ELMT,1)
      ELMOUT(1) = ELMDUM(1)
      ELMOUT(2) = ELMDUM(2)
      DO 9 I=3,6
      ELMDUM(I) = DMOD(ELMDUM(I),PI2)
    9 ELMOUT (I) = ELMDUM(I)*RAD
      TIME= T/TCON
      EMTOUT = ELMT(3) + ELMT(4)
      EMTOUT=DMOD(EMTOUT,PI2)*RAD
C     
C     GET J2 ONLY MEAN ELEMENTS FROM OSCULATING ELEMENTS
C
      CALL KOZJ2(ELMJ2,EMJ2,ELMDUM,-3)
      ELMJ2OT(1) = ELMJ2(1)
      ELMJ2OT(2) = ELMJ2(2)
      DO 10 I=3,6
      ELMJ2(I) = DMOD(ELMJ2(I),PI2)
   10 ELMJ2OT(I) = ELMJ2(I)*RAD
C
C     ELMOUT NOW HAS OSC. ELEMENTS FOR OUTPUT (KM , DEG)
C     ELMTOT HAS  MEAN ELEMENTS FOR OUTPUT (KM,DEG)
C     EMTOUT IS MEAN ARG. OF LATITUDE, I.E. MEAN W + F (DEG)
C     ELMJ2OT HAS J2 ONLY MEAN ELEMENTS FOR OUTPUT
C
C23456789012345678901234567890123456789012345678901234567890123456789012
      IF (KOUT .NE. 0 ) WRITE(6,222) TIME,ELMOUT,ELMTOT,EMTOUT,ELMJ2OT
      IF (KOUT .GT. 0 ) WRITE(*,222) TIME,ELMOUT,ELMTOT,EMTOUT,ELMJ2OT
  222 FORMAT(1H , F8.2,F11.4,F9.7,4F10.4/(1H ,8X,F11.4,F9.7,5F10.4))
C
C     TAB-DELIMITED OUTPUT TO UNIT 99
C
      IF (KU99 .EQ. 0) GO TO 12
      IF(KU99.EQ.1) WRITE (99,555) TIME,Q,ELMOUT(1),Q,ELMOUT(2),Q,
     1     ELMOUT(3),Q,ELMOUT(4),Q,ELMOUT(5),Q,ELMOUT(6)
  555 FORMAT(1H ,F8.2,A1,F11.4,A1,F9.7,4(A1,F10.4))
C
      IF (KU99 .EQ. 4) WRITE(99,555) TIME,Q,ELMJ2OT(1),Q,ELMJ2OT(2),
     1 Q,ELMJ2OT(3),Q,ELMJ2OT(4),Q,ELMJ2OT(5),Q,ELMJ2OT(6)
      IF (KU99 .EQ. 5) WRITE(99,888) TIME,Q,ELMJ2OT(1),Q,ELMJ2OT(2),
     1 Q,ELMJ2OT(3),Q,ELMJ2OT(4),Q,ELMJ2OT(5),Q,ELMJ2OT(6),
     2 Q,ELMOUT(1),Q,ELMOUT(2),Q,ELMOUT(3),Q,ELMOUT(4),Q,ELMOUT(5),
     3 Q,ELMOUT(6)
  888 FORMAT(1H ,F8.2,A1,F11.4,A1,F9.7,4(A1,F10.4),A1,F11.4,
     1 A1, F9.7,4(A1,F10.4) )                
      IF (KU99 .LT.2) GO TO 12
C
C     GET POSITION AND VELOCITY FROM OSCULATING ELEMENTS
C
      ELMDUM(1) = ELMDUM(1)*(1.D0-ELMDUM(2)**2)
      CALL ORBIT(3,-2,XOUT,XDOUT,GM,ELMDUM)
      IF (KU99 .EQ.2) WRITE(99,666) TIME,Q,XOUT(1),Q,XOUT(2),Q
     1     ,XOUT(3),Q,XDOUT(1),Q,XDOUT(2),Q,XDOUT(3)
  666 FORMAT(1H ,F8.2,3(A1,F10.2),3(A1,F10.6))
      IF (KU99 .EQ. 3) WRITE (99,777) TIME,Q,ELMOUT(1),Q,ELMOUT(2)
     1   ,Q,ELMOUT(3),Q,ELMOUT(4),Q,ELMOUT(5),Q,ELMOUT(6),Q,XOUT(1)
     2   ,Q,XOUT(2),Q,XOUT(3),Q,XDOUT(1),Q,XDOUT(2),Q,XDOUT(3)
  777 FORMAT(1H ,F8.2,A1,F11.4,A1,F9.7,4(A1,F10.4),
     1                    3(A1,F10.2),3(A1,F10.6))    
C      
   12 T = T + DELT
      IF ( ABS(T) .LT. ABS(TF+.000001)) GO TO 8
C
      KSE = LONG(362)
      TTIME = KSE - KSS
      TTIME = TTIME/60.
      WRITE(6,*) ' RUN TIME = ', TTIME, ' SECONDS'
      WRITE(*,*) ' RUN TIME = ', TTIME, ' SECONDS'
      IF (KU99 .NE. 0) WRITE(99,*) ' RUN TIME = ', TTIME, ' SECONDS'
C
      IF(NODALP.EQ.0) GO TO 1
C
C     ITERATE FOR NODAL PERIOD
C
      IDIOT = 0
      TN = PI2*NREV/ENBAR
      WRITE(6,*) ' NODAL PERIOD ITERATION '
      WRITE(*,*) ' NODAL PERIOD ITERATION '
   11 DW = 1.5D0*CON*ENBAR*(2.D0-2.5D0*SI*SI)*TN
      IF(J2SQ.NE.0) DW = DW + WD2*TN
      EMB = TRMN(1,-W0,ELMT(2))
      WW = -W0 - DW + PI2*NREV
      EMTOP = TRMN(1,WW,ELMT(2))
      TNN = (EMTOP-EMB)/ENBAR
      DELTN = TNN-TN
      IDIOT = IDIOT + 1
      WRITE(6,333) TNN,TN,DELTN,IDIOT
      WRITE(*,333) TNN,TN,DELTN,IDIOT
  333 FORMAT(1H , 3F15.3,I4)
      IF(IDIOT.GT.10.OR. DABS(DELTN) .LT. 1.D-3) GO TO 1
      TN= TN+DELTN
      GO TO 11
  999 STOP
      END
C
C
      BLOCK DATA
      IMPLICIT REAL*8(A-H,O-Z)  
      COMMON /CONST/ CONST(4)
      COMMON /GRVMOD/ GRVMOD(7)
      DATA CONST /57.29577951308232D0 , 3.141592653589793D0 ,   
     1 6.283185307179586D0 , 149597893.D0 /
      DATA GRVMOD / 398600.8  , 6378.14   ,7.29212361D-5,
     1             1.082D-3 ,-2.56D-6,-1.58D-6,0.D0 /
      END   
C
C
      SUBROUTINE KOZSAK(ELMOUT,EM,ELMIN,MNOSC)
      IMPLICIT REAL*8(A-H,O-Z)
C                           
C     THIS ROUTINE CALCULATES THE SHORT-PERIODIC VARIATIONS IN
C     ORBITAL ELEMENTS DUE TO J2 IN THE MAIN PROBLEM OF THE
C     ARTIFICIAL SATELLITE THEORY, (J2 ONLY). THE NAME IS A
C     COMBINATION OF KOZAI AND IZSAK FROM WHOSE WORK THE
C     FOLLOWING TRANSFORMATION IS DERIVED. THE AUTHOR IS GRATEFUL
C     TO PROF. J.V. BREAKWELL FOR THE REFERENCE TO IZSAK'S NOTE.
C     AKSNES'S WORK FOR LONG-PERIODIC TERMS ADDED 1988 DEC.
C     AKSNES'S FORMULATION INCLUDES J3 AND J4
C            C. UPHOFF 1988/DEC/22
C
C           UNITS ARE KILOMETERS,SECONDS, RADIANS.
C     COORDINATE FRAME IS TRUE EQUATOR AND EQUINOX OF EPOCH.
C
C       MNOSC IS THE FLAG TO TELL WHETHER TO GO FROM MEAN TO
C           OSCULATING OR VICE-VERSA.  IF MNOSC IS NON-NEGATIVE,
C           THE TRANSFORMATION WILL BE FROM MEAN TO OSC, AND
C           NO ITERATION WILL BE PERFORMED. IF MNOSC IS NEGATIVE,
C           THE TRANSFORMATION IS FROM OSC. TO MEAN AND THIS
C           ROUTINE WILL TRY ABS(MNOSC) TIMES TO GET THE MEAN
C           ECCENTRICITY TO CONVERGE TO A VALUE WITHIN TOL
C           OF THE PREVIOUS ITERATION.
C
C     ELMOUT WILL ALWAYS CONTAIN THE TRANSFORMED ELEMENTS,
C     EM IS THE MEAN ANOMALY ASSOCIATED WITH ELMOUT,
C     ELMIN ARE THE INPUT ELEMENTS AND REMAIN UNCHANGED.
C                     GOOD LUCK
C
C     REFERENCES:  BROUWER,D. 1959 ASTRON. J. VOL. 64,P 378.
C                  KOZAI, Y.  1959  IBID.     VOL. 64,P 367.
C                  KOZAI, Y.  1962  IBID.     VOL. 67,P 446.
C                  IZSAK, I.  1963  IBID.     VOL. 68,P 559.
C                  AKSNES,K.  1972 ASTRON. & ASTROPHS. VOL. 17, P 70
C
C
      DIMENSION ELMOUT(6),ELMIN(6)
      COMMON /CONST/ RAD,PI,PI2,AU
      COMMON /GRVMOD/ GM,REQ,SPIN,AJ2,AJ3,AJ4,AJ5
C
      DATA TOL/1.D-7/
C
C
C     AJ2 (J2=-C20) , REQ, AND GM ASSUMED SET IN MAIN OR BLOCK DATA
C
C     NOTE THAT GM IS NOT REQUIRED FOR CONVERSION, ANY VALUE WILL
C     GIVE SAME ANSWERS.....
C
      IDIOT = 1
      SAVE = 1.D10
      DO 1 J=1,6
    1 ELMOUT(J) = ELMIN(J)
C     
      PIN = ELMIN(1)*(1.D0-ELMIN(2)**2)
      SFIN = DSIN(ELMIN(3))
      CFIN = DCOS(ELMIN(3))
      RIN = PIN/(1.D0 + ELMIN(2)*CFIN)
      RDOTIN = DSQRT(GM/PIN)*ELMIN(2)*SFIN
C
C     GET MEAN ANOMALY FROM TRUE ANOMALY
      EM = TRMN(1,ELMOUT(3),ELMOUT(2))
      
C
C     TOP OF LOOP FOR OSC. TO MEAN INTERATION (MNOSC < 0)
C
    2 P = ELMOUT(1) * (1.D0-ELMOUT(2)**2)
      E= ELMOUT(2)
      U= ELMOUT(4) + ELMOUT(3)
C     SET UP SINES AND COSINES
C
      SEPS = DSQRT(1.D0-E*E)
      RMOP = DSQRT(GM/P)
      SU = DSIN(U)
      CU = DCOS(U)
      C2U = DCOS(U+U)
      S2U = DSIN(U+U)
      SI = DSIN (ELMOUT(5))
      CI = DCOS(ELMOUT(5))
      CON = AJ2*REQ*REQ/P/P
C  
C     ADD LONG-PERIODIC STUFF FROM AKSNES,
C          NOTE CON = GAMA
C
      GAM3 = AJ3/(AJ2**2*REQ/P)
      GAM4 = AJ4/AJ2**2
      A1 = 16.D0*GAM3
      A2 = (1.D0-15.D0*CI*CI + 5.D0*GAM4*(1.D0-7.D0*CI*CI))
     1      / (1.D0-5.D0*CI*CI)
      A2PR = (5.D0*A2-15.D0-35.D0*GAM4)/(1.D0 - 5.D0*CI*CI)
C
C     ELEMENTS IN ORDER A,E,F,W,I,NODE,    
C       F = TRUE ANOMALY ,  W=ARG, OF PERIAPSIS
C
      CF = DCOS(ELMOUT(3))
      SF = DSIN(ELMOUT(3))
      CW = DCOS(ELMOUT(4))
      SW = DSIN(ELMOUT(4))
      S2W = 2.D0*SW*CW
      C2W = CW*CW-SW*SW
      C3W = DCOS(3.D0*ELMOUT(4))
C
C     CALC. SINES AND COSINES OF F+2W AND 3F+2W.
C
      SF2W = DSIN(ELMOUT(3)+2.D0*ELMOUT(4))
      CF2W = DCOS(ELMOUT(3)+2.D0*ELMOUT(4))
      S3F2W = DSIN(3.D0*ELMOUT(3)+2.D0*ELMOUT(4))
      C3F2W = DCOS(3.D0*ELMOUT(3)+2.D0*ELMOUT(4))
C
C     CALC. PERTURBATION IN SEMI-LATUS RECTUM,P
C
      DP= P*1.5D0*CON*SI*SI*(C2U+E*CF2W+E*C3F2W/3.D0)
     1     +P*(CON/16.D0)*(-2.D0*A2*SI*SI*E*E*C2W
     2                   +A1*SI*E*SW)
      DELI = 0.5D0*(DP/P)*CI/SI
      DNODE = -1.5D0*CON*CI*(ELMOUT(3)-EM+E*SF-.5D0*S2U
     1                         -.5D0*E*SF2W-E*S3F2W/6.D0)
     2       - (CON*CI/16.D0)*((A2-A2PR*SI*SI)*E*E*S2W
     3                             +0.5D0*A1*E*CW/SI)
C
C     NOW GET SHORT-PERIODIC VARIATION IN RADIUS
C
      DELR = -P*CON/4.D0*((3.D0*CI*CI-1.D0)*(2.D0*SEPS/(1.D0+E*CF)
     1                     +E*CF/(1.D0+SEPS) +1.D0) -SI*SI*C2U)
     2                +(P*CON/32.D0)*(-2.D0*A2*SI*SI*E*CF2W 
     3                               + A1*SI*SU)
C
C     CALC. SHORT PERIODIC VARIATION IN RADIAL SPEED
C
      RTEMP = P/(1.D0 + E*CF)
      DRDOT = 0.25D0*CON*RMOP*((3.D0*CI*CI-1.D0)*E*SF
     1       *(SEPS+(1.D0+E*CF)**2/(1.D0+SEPS))
     2          -2.D0*SI*SI*(1.D0+E*CF)**2*S2U)
     3       -(CON*DSQRT(GM*P)*P/32.D0/RTEMP/RTEMP)*
     4       (-2.D0*A2*SI*SI*E*SF2W - A1*SI*CU)
C     
C     GET VARIATION IN ARG. OF LATITUDE
      
      DELU = -CON/8.D0*(6.D0*(1.D0-5.D0*CI*CI)*(ELMOUT(3)-EM)
     1        +4.D0*((1.D0-6.D0*CI*CI)+(1.D0-3.D0*CI*CI)/
     2        (1.D0+SEPS))*E*SF  +(1.D0-3.D0*CI*CI)/(1.D0+SEPS)
     3        *E*E*2.D0*SF*CF +2.D0*(5.D0*CI*CI-2.D0)*E*SF2W
     4        + (7.D0*CI*CI-1.D0)*S2U + 2.D0*CI*CI*E*S3F2W)
     5       -CON/32.D0*(2.D0*(A2PR*SI*SI-A2)*CI*CI*E*E*S2W
     6         -4.D0*A2*SI*SI*E*SF2W    
     7        -2.D0*A1*SI*CU - A1*(CI*CI/SI/SI - 1.5D0)*SI*E*CW
     7        -A1/2.D0*SI*E*DCOS(U+ELMOUT(3)) )
C
C     PUT 'EM TOGETHER AND WHATTA YA GOT?
C
C
C     BIBBIDY
C
      FLIP = MNOSC
      IF (MNOSC .EQ. 0) FLIP = 1.D0
      FLIP = FLIP/DABS(FLIP)
      POUT = PIN + FLIP*DP
      ROUT = RIN + FLIP * DELR
      RDTOUT = RDOTIN + FLIP * DRDOT
C
C     BOBBIDY
C
      BIGA = POUT/ROUT - 1.D0
      BIGB = RDTOUT/DSQRT(GM/POUT)
      EOUT = DSQRT(BIGA**2 + BIGB**2)
      FOUT = DATAN2(BIGB,BIGA)
C
C     BOO
C
      UOUT= ELMIN(4) + ELMIN(3) + FLIP*DELU
      ELMOUT(1) = POUT/(1.D0-EOUT**2)
      ELMOUT(2) = EOUT
      ELMOUT(5) = ELMIN(5) + FLIP*DELI
      ELMOUT(6) = ELMIN(6) + FLIP * DNODE
      ELMOUT(4) = UOUT -FOUT
      ELMOUT(3) = FOUT
      EM = TRMN(1,ELMOUT(3),ELMOUT(2))
C
C     HERE THEY COME
C
      TEST = DABS(EOUT-SAVE)
      SAVE = EOUT
      IF(MNOSC.GE.-1) RETURN
C
C     THERE THEY GO
C
      IF (MNOSC .LT. 0  .AND. TEST.LT.TOL) GO TO 998
      IF(IDIOT.EQ.ABS(MNOSC)) GO TO 999
      IDIOT = IDIOT + 1
      GO TO 2
  999 WRITE (6,111) TEST,DRDOT,DELR
  111 FORMAT(1H , 'OSCULATING TO MEAN ITERATION FAILS, TEST = '3G12.4)
  998 RETURN
      END
C
C
C
      SUBROUTINE KOZJ2(ELMOUT,EM,ELMIN,MNOSC)
      IMPLICIT REAL*8(A-H,O-Z)
C                           
C     THIS ROUTINE CALCULATES THE SHORT-PERIODIC VARIATIONS IN
C     ORBITAL ELEMENTS DUE TO J2 IN THE MAIN PROBLEM OF THE
C     ARTIFICIAL SATELLITE THEORY, (J2 ONLY). THE NAME IS A
C     COMBINATION OF KOZAI AND IZSAK FROM WHOSE WORK THE
C     FOLLOWING TRANSFORMATION IS DERIVED. THE AUTHOR IS GRATEFUL
C     TO PROF. J.V. BREAKWELL FOR THE REFERENCE TO IZSAK'S NOTE.
C     AKSNES'S WORK FOR LONG-PERIODIC TERMS ADDED 1988 DEC.
C            C. UPHOFF 1988/DEC/22
C
C           UNITS ARE KILOMETERS,SECONDS, RADIANS.
C     COORDINATE FRAME IS TRUE EQUATOR AND EQUINOX OF EPOCH.
C
C       MNOSC IS THE FLAG TO TELL WHETHER TO GO FROM MEAN TO
C           OSCULATING OR VICE-VERSA.  IF MNOSC IS NON-NEGATIVE,
C           THE TRANSFORMATION WILL BE FROM MEAN TO OSC, AND
C           NO ITERATION WILL BE PERFORMED. IF MNOSC IS NEGATIVE,
C           THE TRANSFORMATION IS FROM OSC. TO MEAN AND THIS
C           ROUTINE WILL TRY ABS(MNOSC) TIMES TO GET THE MEAN
C           ECCENTRICITY TO CONVERGE TO A VALUE WITHIN TOL
C           OF THE PREVIOUS ITERATION.
C
C     ELMOUT WILL ALWAYS CONTAIN THE TRANSFORMED ELEMENTS,
C     EM IS THE MEAN ANOMALY ASSOCIATED WITH ELMOUT,
C     ELMIN ARE THE INPUT ELEMENTS AND REMAIN UNCHANGED.
C                     GOOD LUCK
C
C     REFERENCES:  BROUWER,D. 1959 ASTRON. J. VOL. 64,P 378.
C                  KOZAI, Y.  1959  IBID.     VOL. 64,P 367.
C                  KOZAI, Y.  1962  IBID.     VOL. 67,P 446.
C                  IZSAK, I.  1963  IBID.     VOL. 68,P 559.
C                  AKSNES,K.  1972 ASTRON. & ASTROPHS. VOL. 17, P 70
C
C
      DIMENSION ELMOUT(6),ELMIN(6)
      COMMON /CONST/ RAD,PI,PI2,AU
      COMMON /GRVMOD/ GM,REQ,SPIN,AJ2,AJ3,AJ4,AJ5
C
      DATA TOL/1.D-7/
C
C
C     AJ2 (J2=-C20) , REQ, AND GM ASSUMED SET IN MAIN OR BLOCK DATA
C
C     NOTE THAT GM IS NOT REQUIRED FOR CONVERSION, ANY VALUE WILL
C     GIVE SAME ANSWERS.....
C
      IDIOT = 1
      SAVE = 1.D10
      DO 1 J=1,6
    1 ELMOUT(J) = ELMIN(J)
C     
      PIN = ELMIN(1)*(1.D0-ELMIN(2)**2)
      SFIN = DSIN(ELMIN(3))
      CFIN = DCOS(ELMIN(3))
      RIN = PIN/(1.D0 + ELMIN(2)*CFIN)
      RDOTIN = DSQRT(GM/PIN)*ELMIN(2)*SFIN
C
C     GET MEAN ANOMALY FROM TRUE ANOMALY
      EM = TRMN(1,ELMOUT(3),ELMOUT(2))
      
C
C     TOP OF LOOP FOR OSC. TO MEAN INTERATION (MNOSC < 0)
C
    2 P = ELMOUT(1) * (1.D0-ELMOUT(2)**2)
      E= ELMOUT(2)
      U= ELMOUT(4) + ELMOUT(3)
C     SET UP SINES AND COSINES
C
      SEPS = DSQRT(1.D0-E*E)
      RMOP = DSQRT(GM/P)
      SU = DSIN(U)
      CU = DCOS(U)
      C2U = DCOS(U+U)
      S2U = DSIN(U+U)
      SI = DSIN (ELMOUT(5))
      CI = DCOS(ELMOUT(5))
      CON = AJ2*REQ*REQ/P/P
C  
C     ADD LONG-PERIODIC STUFF FROM AKSNES,
C          NOTE CON = GAMA
C
      GAM3 = 0.D0
      GAM4 = 0.D0
      A1 = 16.D0*GAM3
      A2 = 0.D0
      A2PR = 0.D0
C
C     ELEMENTS IN ORDER A,E,F,W,I,NODE,    
C       F = TRUE ANOMALY ,  W=ARG, OF PERIAPSIS
C
      CF = DCOS(ELMOUT(3))
      SF = DSIN(ELMOUT(3))
      CW = DCOS(ELMOUT(4))
      SW = DSIN(ELMOUT(4))
      S2W = 2.D0*SW*CW
      C2W = CW*CW-SW*SW
      C3W = DCOS(3.D0*ELMOUT(4))
C
C     CALC. SINES AND COSINES OF F+2W AND 3F+2W.
C
      SF2W = DSIN(ELMOUT(3)+2.D0*ELMOUT(4))
      CF2W = DCOS(ELMOUT(3)+2.D0*ELMOUT(4))
      S3F2W = DSIN(3.D0*ELMOUT(3)+2.D0*ELMOUT(4))
      C3F2W = DCOS(3.D0*ELMOUT(3)+2.D0*ELMOUT(4))
C
C     CALC. PERTURBATION IN SEMI-LATUS RECTUM,P
C
      DP= P*1.5D0*CON*SI*SI*(C2U+E*CF2W+E*C3F2W/3.D0)
     1     +P*(CON/16.D0)*(-2.D0*A2*SI*SI*E*E*C2W
     2                   +A1*SI*E*SW)
      DELI = 0.5D0*(DP/P)*CI/SI
      DNODE = -1.5D0*CON*CI*(ELMOUT(3)-EM+E*SF-.5D0*S2U
     1                         -.5D0*E*SF2W-E*S3F2W/6.D0)
     2       - (CON*CI/16.D0)*((A2-A2PR*SI*SI)*E*E*S2W
     3                             +0.5D0*A1*E*CW/SI)
C
C     NOW GET SHORT-PERIODIC VARIATION IN RADIUS
C
      DELR = -P*CON/4.D0*((3.D0*CI*CI-1.D0)*(2.D0*SEPS/(1.D0+E*CF)
     1                     +E*CF/(1.D0+SEPS) +1.D0) -SI*SI*C2U)
     2                +(P*CON/32.D0)*(-2.D0*A2*SI*SI*E*CF2W 
     3                               + A1*SI*SU)
C
C     CALC. SHORT PERIODIC VARIATION IN RADIAL SPEED
C
      RTEMP = P/(1.D0 + E*CF)
      DRDOT = 0.25D0*CON*RMOP*((3.D0*CI*CI-1.D0)*E*SF
     1       *(SEPS+(1.D0+E*CF)**2/(1.D0+SEPS))
     2          -2.D0*SI*SI*(1.D0+E*CF)**2*S2U)
     3       -(CON*DSQRT(GM*P)*P/32.D0/RTEMP/RTEMP)*
     4       (-2.D0*A2*SI*SI*E*SF2W - A1*SI*CU)
C     
C     GET VARIATION IN ARG. OF LATITUDE
      
      DELU = -CON/8.D0*(6.D0*(1.D0-5.D0*CI*CI)*(ELMOUT(3)-EM)
     1        +4.D0*((1.D0-6.D0*CI*CI)+(1.D0-3.D0*CI*CI)/
     2        (1.D0+SEPS))*E*SF  +(1.D0-3.D0*CI*CI)/(1.D0+SEPS)
     3        *E*E*2.D0*SF*CF +2.D0*(5.D0*CI*CI-2.D0)*E*SF2W
     4        + (7.D0*CI*CI-1.D0)*S2U + 2.D0*CI*CI*E*S3F2W)
     5       -CON/32.D0*(2.D0*(A2PR*SI*SI-A2)*CI*CI*E*E*S2W
     6         -4.D0*A2*SI*SI*E*SF2W    
     7        -2.D0*A1*SI*CU - A1*(CI*CI/SI/SI - 1.5D0)*SI*E*CW
     7        -A1/2.D0*SI*E*DCOS(U+ELMOUT(3)) )
C
C     PUT 'EM TOGETHER AND WHATTA YA GOT?
C
C
C     BIBBIDY
C
      FLIP = MNOSC
      IF (MNOSC .EQ. 0) FLIP = 1.D0
      FLIP = FLIP/DABS(FLIP)
      POUT = PIN + FLIP*DP
      ROUT = RIN + FLIP * DELR
      RDTOUT = RDOTIN + FLIP * DRDOT
C
C     BOBBIDY
C
      BIGA = POUT/ROUT - 1.D0
      BIGB = RDTOUT/DSQRT(GM/POUT)
      EOUT = DSQRT(BIGA**2 + BIGB**2)
      FOUT = DATAN2(BIGB,BIGA)
C
C     BOO
C
      UOUT= ELMIN(4) + ELMIN(3) + FLIP*DELU
      ELMOUT(1) = POUT/(1.D0-EOUT**2)
      ELMOUT(2) = EOUT
      ELMOUT(5) = ELMIN(5) + FLIP*DELI
      ELMOUT(6) = ELMIN(6) + FLIP * DNODE
      ELMOUT(4) = UOUT -FOUT
      ELMOUT(3) = FOUT
      EM = TRMN(1,ELMOUT(3),ELMOUT(2))
C
C     HERE THEY COME
C
      TEST = DABS(EOUT-SAVE)
      SAVE = EOUT
      IF(MNOSC.GE.-1) RETURN
C
C     THERE THEY GO
C
      IF (MNOSC .LT. 0  .AND. TEST.LT.TOL) GO TO 998
      IF(IDIOT.EQ.ABS(MNOSC)) GO TO 999
      IDIOT = IDIOT + 1
      GO TO 2
  999 WRITE (6,111) TEST,DRDOT,DELR
  111 FORMAT(1H , 'OSCULATING TO MEAN ITERATION FAILS, TEST = '3G12.4)
  998 RETURN
      END
C
C
      SUBROUTINE CROSS (X,Y,Z)  
      IMPLICIT REAL*8(A-H,O-Z)  
      DIMENSION X(3),Y(3),Z(3),DUM(3)  
      DUM(1) = X(2)*Y(3) - X(3)*Y(2)  
      DUM(2) = X(3)*Y(1) - X(1)*Y(3)  
      DUM(3) = X(1)*Y(2) - X(2)*Y(1)
      DO 1 J = 1,3
    1 Z(J) = DUM(J)
      RETURN
      END
C

      DOUBLE PRECISION FUNCTION DARCOS(X)   
      DOUBLE PRECISION X
      DARCOS = DACOS(X) 
      RETURN
      END
C
C
      DOUBLE PRECISION FUNCTION DARSIN(X)   
      DOUBLE PRECISION X
      DARSIN = DASIN(X) 
      RETURN
      END
C
C
      DOUBLE PRECISION FUNCTION DOT (X,Y)   
      IMPLICIT REAL*8(A-H,O-Z)  
      DIMENSION X(3),Y(3)   
      DOT = X(1)*Y(1) + X(2)*Y(2) + X(3)*Y(3)   
      RETURN
      END
C
C
      DOUBLE PRECISION FUNCTION DVMAG(X)
      IMPLICIT REAL*8(A-H,O-Z)  
C........FUNCTION  DVMAG CALCULATES THE MAGNITUDE OF A DOUBLE PRECISION 
C         VECTOR
      DIMENSION X(3)
      DVMAG =  DSQRT(X(1)**2 + X(2)**2 + X(3)**2 )  
      RETURN
      END
C
C
      SUBROUTINE INRTRT(ROT,PHI)
      IMPLICIT REAL*8(A-H,O-Z)
C     SETS UP ROT. MAT. FOR XFM FROM INERTIAL TO ROTATING FRAME
      DIMENSION ROT(3,3)
      CPHI = DCOS(PHI)
      SPHI = DSIN(PHI)
      ROT(1,1) = CPHI
      ROT(1,2) = SPHI
      ROT(1,3) = 0.D0
      ROT(2,1) = -SPHI
      ROT(2,2) = CPHI
      ROT(2,3) = 0.D0
      ROT(3,1) = 0.D0
      ROT(3,2) = 0.D0
      ROT(3,3) = 1.D0
      RETURN
      END
C
C
      SUBROUTINE MVTRN(A,B,C,M,N)   
      IMPLICIT REAL*8(A-H,O-Z)  
      DIMENSION A(9),B(1),C(1)  
      N3 = 3*N  
      DO 5 I=1,N3,3 
      B1 = B(I) 
      B2 = B(I+1)   
      B3 = B(I+2)
      IF (M .NE. 1) GO TO 3    
C           C = A B 
    2 D1 = A(1)*B1 + A(4)*B2 + A(7)*B3  
      D2 = A(2)*B1 + A(5)*B2 + A(8)*B3  
      D3 = A(3)*B1 + A(6)*B2 + A(9)*B3  
      GO TO 4   
C          C = A(TRANSPOSED) B  
    3 D1 = A(1)*B1 + A(2)*B2 + A(3)*B3  
      D2 = A(4)*B1 + A(5)*B2 + A(6)*B3  
      D3 = A(7)*B1 + A(8)*B2 + A(9)*B3  
    4 C(I) = D1 
      C(I+1) = D2   
      C(I+2) = D3   
    5 CONTINUE  
      RETURN
      END
C
C
      SUBROUTINE ORBIT(K,J,X,XD,GM,ELM) 
      IMPLICIT REAL*8(A-H,O-Z)  
C          REPLACES ORBIT3-5-6 FOR K=3, 5 OR 6  
C          ORBIT5 MUST SPLIT X INTO X,X(4) IN ARGUMENT LIST FOR ORBIT   
      DIMENSION X(3),XD(3),ELM(6)   
      COMMON /CONST/ RAD,PI,PI2,AU  
      IF (J .LT. 0) GO TO 111
C
C     GET ELEMENTS FROM POSITION AND VELOCITY
C
   11 R= DSQRT(X(1)*X(1) + X(2)*X(2) + X(3)*X(3))   
      RDOT = (X(1)*XD(1) +X(2)*XD(2)+X(3)*XD(3))/ R 
      WXP = X(2)*XD(3) - X(3)*XD(2) 
      WYP = X(3)*XD(1) - X(1)*XD(3) 
      WZP = X(1)*XD(2) - X(2)*XD(1) 
      P = (WXP*WXP + WYP*WYP + WZP*WZP) / GM
      H = DSQRT(P*GM)   
      SU = RDOT * P / H 
      CU = (P - R)/R
      E = DSQRT(CU**2 + SU**2)  
      WZ = WZP/H
      IF (DABS(WZ) .GE. 1.D0) GO TO 2   
    1 CI = DARCOS(WZ)   
      O = DATAN2(WXP,(-WYP))
      SN = X(3)*H*(1.0D0-WZ*WZ) - WZ*(X(1)*WXP + X(2)*WYP)  
      CN = X(2)*WXP - X(1)*WYP  
      UN   = DATAN2(   SN,CN)   
      GO TO 3   
2     CI = DATAN2(0.D0,WZ)  
      O = 0.0D0 
      UN = DATAN2((X(2)*WZ), X(1))  
    3 IF (E .LE. 2.D-7) GO TO 7 
    4 U = DATAN2(SU,CU) 
      W = UN - U
      IF (W .LT. 0.D0) W = W + PI2  
      GO TO 8   
    7 W = 0.0D0 
      U = UN
    8 ELM(1) = P
      ELM(5) = CI   
      ELM(6) = O
      IF (K .NE. 6) GO TO 300   
      ELM(2) = E * DSIN(W)  
      ELM(3) = E * DCOS(W)  
      ELM(4) = W + U
      RETURN
300   ELM(2) = E
      ELM(3) = U
      ELM(4) = W
      IF (K .LT. 4) RETURN  
      ELM(7) = DCOS(W)  
      ELM(8) = DSIN(W)  
      ELM(9) = DCOS(CI) 
      ELM(10)= DSIN(CI) 
      ELM(11) = DCOS(O) 
      ELM(12) = DSIN(O) 
      RETURN
C
C     GET POSITION AND VELOCITY (J = -2) FROM ELEMENTS
C     (J = -1) RETURNS POSITION ONLY
C
111   IF (K - 5) 333,555,666
555   CW = ELM(7)   
      SW = ELM(8)   
      CINC = ELM(9) 
      SINC = ELM(10)
      CO = ELM(11)  
      SO = ELM(12)  
      GO TO 334 
333   CW = DCOS(ELM(4)) 
      SW = DSIN(ELM(4)) 
666   CINC = DCOS(ELM(5))   
      CO = DCOS(ELM(6)) 
      SINC = DSIN(ELM(5))   
      SO = DSIN(ELM(6)) 
      IF (K .GT. 5) GO TO 667   
334   E =ELM(2) 
      U = ELM(3)
      GO TO 668 
667   E = DSQRT(ELM(2)**2 + ELM(3)**2)  
      W = DATAN2(ELM(2), ELM(3))
      SW = ELM(2)/E 
      CW = ELM(3)/E 
      U = ELM(4) - W
  668 P = ELM(1)
      CU = DCOS(U)  
      SU = DSIN(U)  
      A11 = CW * CO - CINC*SO * SW  
      A12 = -SW*CO  - CINC*SO * CW  
      A21 =  CW*SO  + CINC*CO * SW  
      A22 = -SW*SO  + CINC*CO * CW  
      A31 =  SINC * SW  
      A32 =  SINC * CW  
      IF(J.EQ.-3) GO TO 66  
      R = ELM(1)/(1.D0 + E*CU)  
      XPR = R*CU
      YPR = R*SU
      X(1) = A11*XPR + A12*YPR  
      X(2) = A21*XPR + A22*YPR  
      X(3) = A31*XPR + A32*YPR  
      IF (J .EQ. -1) RETURN 
   12 V =  DSQRT(GM *(2.D0/R - (1.D0 - E*E)/ELM(1)))
      GAMA = DATAN(E*SU/(1.D0 + E*CU))  
67    VXPR = -V * DSIN(U-GAMA)  
      VYPR = V* DCOS(U-GAMA)
      XD(1) = A11*VXPR  +  A12*VYPR 
      XD(2) = A21*VXPR  +  A22*VYPR 
      XD(3) = A31*VXPR  +  A32*VYPR 
   13 RETURN
66    V = DSQRT(GM*(E**2 - 1.D0)/ELM(1))
      GAMA = PI2/4.D0   
      GO TO 67  
      END
C
C
      SUBROUTINE ROTATE (M,A,B,C)  
      IMPLICIT REAL*8(A-H,O-Z)  
      DIMENSION A(9),B(3),C(3)       
C        ROTATE DOES MATRIX-VECTOR MULT 
      IF (M .NE. 1) GO TO 3    
C           C = A B 
    2 C(1) = A(1)*B(1) + A(4)*B(2) + A(7)*B(3)  
      C(2) = A(2)*B(1) + A(5)*B(2) + A(8)*B(3)  
      C(3) = A(3)*B(1) + A(6)*B(2) + A(9)*B(3)  
      GO TO 4   
C          C = A(TRANSPOSED) B  
    3 C(1) = A(1)*B(1) + A(2)*B(2) + A(3)*B(3)  
      C(2) = A(4)*B(1) + A(5)*B(2) + A(6)*B(3)  
      C(3) = A(7)*B(1) + A(8)*B(2) + A(9)*B(3)  
    4 CONTINUE
      RETURN
      END
C
C
      DOUBLE PRECISION FUNCTION TRMN (J,QQ,E)
      IMPLICIT REAL*8(A-H,O-Z)  
      COMMON /CONST/ RAD,PI,PI2,AU  
      F = QQ 
      D = DABS(1.D0-E)  
      U = F 
      IF(J)2,1,1
    1 IF (E.GT.1.D0) GO TO 20   
      CF =  DCOS(F)
      IF (DABS(F).LT. 1.D-8) CF = 1.D0 - F*F/2.D0 + F**4/24.D0
      ECAN = DARCOS((E + CF)/(1.D0 + E * CF))   
      MTST = F/PI   
      TST = MTST
      ONE = 1.D0
      IF (F .LT. 0.D0) ONE = -ONE   
      MTST = MOD(MTST,2)
      IF (MTST .NE. 0) ECAN = TST * PI + ONE*(PI-ECAN)  
      IF (MTST .EQ. 0) ECAN = TST * PI + ONE*ECAN   
      TRMN = ECAN - E *  DSIN(ECAN) 
      RETURN
   20 ULIM = PI + DARCOS(1.D0/E)
      BLIM = PI2 - ULIM 
      IF(F.GE.BLIM.AND.F.LE.ULIM) GO TO 21  
      FARG =  DSQRT((E - 1.D0)/(E + 1.D0)) *  DSIN(F/2.D0)/ DCOS(F/2.D0)
      FAN =   DLOG (1.D0 + FARG) -   DLOG(1.D0 - FARG)  
      TRMN = -FAN + E *  DSINH(FAN) 
      RETURN
   21 WRITE(6,111)  
  111 FORMAT(1H ,26HTRUE ANOMALY NOT POSSIBLE ) 
      TRMN = 0.0D0  
      RETURN
    2 IF(E-1.D0) 5,6,7  
    5 U = F 
      DO 8 M=1,50   
      DEM = F - U + E *  DSIN(U)
      ECOSU = E *  DCOS(U)  
      DELU = DEM /(1.D0 - ECOSU + .01D0*ECOSU**3)   
      IF (ECOSU .GT. .99D0 .OR. M .GT. 10   ) DELU = DEM/(1.D0 - ECOSU) 
      IF(DABS(DELU).GT. 2.D0) DELU=DSIGN(2.D0,DELU) 
      U = U + DELU  
      IF ( DABS(DEM) - 2.D-12)9,9,8 
    8 CONTINUE  
      WRITE (6,119) QQ,E,DEM 
  119 FORMAT (23H TRMN FAILS, E.LT. 1.0    3D10.4 ) 
    9 COSU =  DCOS(U)   
      DEM1 = 1.D0 - E*COSU  
      CS =(COSU -E) /DEM1   
      SS =  DSQRT(1.D0 -E*E) *  DSIN(U)/DEM1
      GO TO 99  
    6 E = E + .0001D0   
      GO TO 2   
    7 U = F/D   
      IF(D.LE.DABS(F) ) U=DSIGN((6.D0*DABS(F))**(1.D0/3.D0),F)  
      IF(DABS(F).GT.1.D0)U=DSIGN(DLOG(1.D0+DABS(2.D0*F/E)),F)   
      DO 12 L=1,100 
      DEM = F + U - E *  DSINH(U)   
      ECOSU = E *  DCOSH(U) 
      DELU =-DEM / (1.D0-ECOSU) 
      U = U + DELU  
      IF (DABS(DEM) - 2.D-12) 11,11,12  
   12 CONTINUE  
      WRITE (6,120)QQ,E,DEM  
  120 FORMAT(23H TRMN FAILS, E.GT. 1.0      3D10.4  )   
   11 COSU =  DCOSH(U)  
      DEM1 = 1.D0 - E*COSU  
      CS = (COSU - E) / DEM1
      SS = - DSQRT(E*E - 1.D0) *  DSINH(U)/DEM1 
   99 TRMN = DATAN2(SS,CS)  
      RETURN
      END 
