

*************************************************************
*
*     Routines to compute the NPOINT track intersection points
*     with planes of z-coordinate given by ZIN
*     given the track parameters
*
*     The routine is based on GRKUTA, which computes the 
*     trajectory of a charged particle in a magnetic field
*     by solving the equations of motion with Runge-Kuta method
*
*     Variables that have to be assigned when the subroutine 
*     is called:
*
*     ZIN(1-NPOINT) ----> z coordinates of the planes
*     AL_P(1-5)     ----> track-parameter vector
*
*     NB !!! 
*     The routine works properly only if the 
*     planes are numbered in descending order 
*     
*     Routines:
*
*     DOTRACK(NPOINT,ZIN,XOUT,YOUT,AL_P,IFAIL)   
*     - old routine
*     
*     DOTRACK2(NPOINT,ZIN,XOUT,YOUT,THXOUT,THYOUT,TLOUT,AL_P,IFAIL)
*     - as the previous one, but: 
*     -- the projected angles are given as output
*     -- the track lengths are given as output:
*     ---- for planes above the reference plane, the length until 
*          the lower closest plane (reference plane included)
*     ---- for planes below the reference plane, the length until 
*          the higher closest plane (reference plane included)
*
*     March 2008 --> optimized by Paolo
*
*     DOTRACK3(NPOINT,ZIN,XOUT,YOUT,THXOUT,THYOUT,TLOUT,AL_P,ZINI,IFAIL)
*     - as the previous one, but with the coordinate ZINI of the reference
*       plane given as input parameter
*
*
**************************************************************


      SUBROUTINE 
     $     DOTRACK3(NPOINT,ZIN,XOUT,YOUT,THXOUT,THYOUT,TLOUT,AL_P
     $     ,ZINI,IFAIL)

      IMPLICIT DOUBLE PRECISION (A-H,O-Z)


      DIMENSION VECT(7),VECTINI(7),VOUT(7)
      DATA TOLL/1.d-8/          

*     tolerance in reaching the next plane during the tracking procedure
*     -----------------------------------------------
*     I/O parameters
      PARAMETER (NPOINT_MAX=500)
      DIMENSION ZIN(NPOINT_MAX)
      DIMENSION XOUT(NPOINT_MAX),YOUT(NPOINT_MAX)
      DIMENSION THXOUT(NPOINT_MAX),THYOUT(NPOINT_MAX)
      DIMENSION TLOUT(NPOINT_MAX)
      DIMENSION AL_P(5)
*     -----------------------------------------------      
*      DATA ZINI/23.5/           !z coordinate of the reference plane
      REAL*8 L

*     ==================================================================
*     divide the track in two parts: below and above the reference plane
*     ==================================================================      
      IUPDOWN=0
      DO I=1,NPOINT   
         IF(ZIN(I).LT.ZINI)THEN
            if(i.ne.1)IUPDOWN=I
            GOTO 88
         ENDIF
         IUPDOWN=NPOINT+1
      ENDDO
 88   CONTINUE

*     ==================================================================
*     track from reference plane DOWN
*     ==================================================================
*     parameters for GRKUTA
      IF(AL_P(5).NE.0) CHARGE=AL_P(5)/DABS(AL_P(5))
      IF(AL_P(5).EQ.0) CHARGE=1.
      VOUT(1)=AL_P(1)
      VOUT(2)=AL_P(2)
      VOUT(3)=ZINI             
      VOUT(4)=AL_P(3)*DCOS(AL_P(4))
      VOUT(5)=AL_P(3)*DSIN(AL_P(4))
      VOUT(6)=-1.*DSQRT(1.-AL_P(3)**2)
      IF(AL_P(5).NE.0.) VOUT(7)=DABS(1./AL_P(5))
      IF(AL_P(5).EQ.0.) VOUT(7)=1.E8
      DO I=MAX(IUPDOWN,1),NPOINT
         L = 0.0
 10      DO J=1,7
            VECT(J)=VOUT(J)
            VECTINI(J)=VOUT(J)
         ENDDO
         IF(VOUT(6).GE.0.) THEN
            IFAIL=1
c$$$            if(TRKVERBOSE)      
c$$$     $           PRINT *,'dofit (grkuta): WARNING ===> backward track!!'
            RETURN
         ENDIF
         step=(zin(i)-vect(3))/VOUT(6)
 11      continue
         CALL GRKUTA(CHARGE,STEP,VECT,VOUT)
         L = L + STEP
         IF(VOUT(3).GT.VECT(3)) THEN
            IFAIL=1
c$$$            if(TRKVERBOSE)
c$$$     $      PRINT *,'dofit (grkuta): WARNING ===> backward track!!'
c$$$            if(TRKVERBOSE)print*,'charge',charge
c$$$            if(TRKVERBOSE)print*,'vect',vect
c$$$            if(TRKVERBOSE)print*,'vout',vout
c$$$            if(TRKVERBOSE)print*,'step',step
c$$$            if(TRKVERBOSE)print*,'DeltaB',DELTA0,DELTA1
            RETURN
         ENDIF
         Z=VOUT(3)
         IF(Z.LE.ZIN(I)+TOLL.AND.Z.GE.ZIN(I)-TOLL) GOTO 100
         IF(Z.GT.ZIN(I)+TOLL) GOTO 10      
         IF(Z.LE.ZIN(I)-TOLL) THEN
            L = L - STEP
            STEP=STEP*(ZIN(I)-VECT(3))/(Z-VECT(3))
            DO J=1,7
               VECT(J)=VECTINI(J)
            ENDDO
            GOTO 11
         ENDIF
 100     XOUT(I)=VOUT(1)
         YOUT(I)=VOUT(2)
         ZIN(I)=VOUT(3)
         IF(VOUT(3).ne.0)THEN
            THXOUT(I)=DATAN(VOUT(4)/VOUT(6))*180./ACOS(-1.)
            THYOUT(I)=DATAN(VOUT(5)/VOUT(6))*180./ACOS(-1.)
         ENDIF
         TLOUT(I) = L
c         print*,'D ',VECT(3),VOUT(3),VOUT(8),STEP
      ENDDO

*     ==================================================================
*     track from reference plane UP
*     ==================================================================
*     parameters for GRKUTA:
*     -opposite charge
*     -opposite momentum direction
      IF(AL_P(5).NE.0) CHARGE=-AL_P(5)/DABS(AL_P(5))
      IF(AL_P(5).EQ.0) CHARGE=-1.
      VOUT(1)=AL_P(1)
      VOUT(2)=AL_P(2)
      VOUT(3)=ZINI             
      VOUT(4)=-AL_P(3)*DCOS(AL_P(4))
      VOUT(5)=-AL_P(3)*DSIN(AL_P(4))
      VOUT(6)=1.*DSQRT(1.-AL_P(3)**2)
      IF(AL_P(5).NE.0.) VOUT(7)=DABS(1./AL_P(5))
      IF(AL_P(5).EQ.0.) VOUT(7)=1.E8
      DO I=MIN((IUPDOWN-1),NPOINT),1,-1
         L = 0.0

 20      DO J=1,7
            VECT(J)=VOUT(J)
            VECTINI(J)=VOUT(J)
         ENDDO
         IF(VOUT(6).LE.0.) THEN
            IFAIL=1
c$$$            if(TRKVERBOSE)      
c$$$     $           PRINT *,'dofit (grkuta): WARNING ===> backward track!!'
            RETURN
         ENDIF
         step=(zin(i)-vect(3))/VOUT(6)
 22      continue
         CALL GRKUTA(CHARGE,STEP,VECT,VOUT)
         L = L + STEP
         IF(VOUT(3).LT.VECT(3)) THEN
            IFAIL=1
c$$$            if(TRKVERBOSE)
c$$$     $      PRINT *,'dofit (grkuta): WARNING ===> backward track!!'
c$$$            if(TRKVERBOSE)print*,'charge',charge
c$$$            if(TRKVERBOSE)print*,'vect',vect
c$$$            if(TRKVERBOSE)print*,'vout',vout
c$$$            if(TRKVERBOSE)print*,'step',step
c$$$            if(TRKVERBOSE)print*,'DeltaB',DELTA0,DELTA1
            RETURN
         ENDIF
         Z=VOUT(3)
         IF(Z.LE.ZIN(I)+TOLL.AND.Z.GE.ZIN(I)-TOLL) GOTO 200
         IF(Z.LT.ZIN(I)-TOLL) GOTO 20      
         IF(Z.GE.ZIN(I)+TOLL) THEN
            L = L - STEP
            STEP=STEP*(ZIN(I)-VECT(3))/(Z-VECT(3))
            DO J=1,7
               VECT(J)=VECTINI(J)
            ENDDO
            GOTO 22
         ENDIF
 200     XOUT(I)=VOUT(1)
         YOUT(I)=VOUT(2)
         ZIN(I)=VOUT(3)
         IF(VOUT(3).ne.0)THEN
            THXOUT(I)=DATAN(VOUT(4)/VOUT(6))*180./ACOS(-1.)
            THYOUT(I)=DATAN(VOUT(5)/VOUT(6))*180./ACOS(-1.)
         ENDIF
         TLOUT(I) = L
      ENDDO

      RETURN
      END
************************************************************************
*
*
*
*
************************************************************************

      SUBROUTINE 
     $     DOTRACK2(NPOINT,ZIN,XOUT,YOUT,THXOUT,THYOUT,TLOUT,AL_P,IFAIL)

      IMPLICIT DOUBLE PRECISION (A-H,O-Z)


ccc      DIMENSION VECT(7),VECTINI(7),VOUT(7)
ccc      DATA TOLL/1.d-8/          

*     tolerance in reaching the next plane during the tracking procedure
*     -----------------------------------------------
*     I/O parameters
      PARAMETER (NPOINT_MAX=500)
      DIMENSION ZIN(NPOINT_MAX)
      DIMENSION XOUT(NPOINT_MAX),YOUT(NPOINT_MAX)
      DIMENSION THXOUT(NPOINT_MAX),THYOUT(NPOINT_MAX)
      DIMENSION TLOUT(NPOINT_MAX)
      DIMENSION AL_P(5)
*     -----------------------------------------------      
      DATA ZINI/23.5/           !z coordinate of the reference plane
ccc      REAL*8 L

      CALL DOTRACK3(NPOINT,ZIN,XOUT,YOUT,THXOUT,THYOUT,TLOUT,AL_P
     $     ,ZINI,IFAIL)

ccc
ccc   OLD-OLD-OLD-OLD-OLD-OLD-OLD-OLD-OLD-OLD-OLD-OLD-OLD-OLD-OLD-OLD-OLD-OLD
ccc

c$$$*     ==================================================================
c$$$*     divide the track in two parts: below and above the reference plane
c$$$*     ==================================================================      
c$$$      IUPDOWN=0
c$$$      DO I=1,NPOINT   
c$$$         IF(ZIN(I).LT.ZINI)THEN
c$$$            if(i.ne.1)IUPDOWN=I
c$$$            GOTO 88
c$$$         ENDIF
c$$$         IUPDOWN=NPOINT+1
c$$$      ENDDO
c$$$ 88   CONTINUE
c$$$
c$$$*     ==================================================================
c$$$*     track from reference plane DOWN
c$$$*     ==================================================================
c$$$*     parameters for GRKUTA
c$$$      IF(AL_P(5).NE.0) CHARGE=AL_P(5)/DABS(AL_P(5))
c$$$      IF(AL_P(5).EQ.0) CHARGE=1.
c$$$      VOUT(1)=AL_P(1)
c$$$      VOUT(2)=AL_P(2)
c$$$      VOUT(3)=ZINI             
c$$$      VOUT(4)=AL_P(3)*DCOS(AL_P(4))
c$$$      VOUT(5)=AL_P(3)*DSIN(AL_P(4))
c$$$      VOUT(6)=-1.*DSQRT(1.-AL_P(3)**2)
c$$$      IF(AL_P(5).NE.0.) VOUT(7)=DABS(1./AL_P(5))
c$$$      IF(AL_P(5).EQ.0.) VOUT(7)=1.E8
c$$$      DO I=MAX(IUPDOWN,1),NPOINT
c$$$         L = 0.0
c$$$ 10      DO J=1,7
c$$$            VECT(J)=VOUT(J)
c$$$            VECTINI(J)=VOUT(J)
c$$$         ENDDO
c$$$         IF(VOUT(6).GE.0.) THEN
c$$$            IFAIL=1
c$$$c$$$            if(TRKVERBOSE)      
c$$$c$$$     $           PRINT *,'dofit (grkuta): WARNING ===> backward track!!'
c$$$            RETURN
c$$$         ENDIF
c$$$         step=(zin(i)-vect(3))/VOUT(6)
c$$$ 11      continue
c$$$         CALL GRKUTA(CHARGE,STEP,VECT,VOUT)
c$$$         L = L + STEP
c$$$         IF(VOUT(3).GT.VECT(3)) THEN
c$$$            IFAIL=1
c$$$c$$$            if(TRKVERBOSE)
c$$$c$$$     $      PRINT *,'dofit (grkuta): WARNING ===> backward track!!'
c$$$c$$$            if(TRKVERBOSE)print*,'charge',charge
c$$$c$$$            if(TRKVERBOSE)print*,'vect',vect
c$$$c$$$            if(TRKVERBOSE)print*,'vout',vout
c$$$c$$$            if(TRKVERBOSE)print*,'step',step
c$$$c$$$            if(TRKVERBOSE)print*,'DeltaB',DELTA0,DELTA1
c$$$            RETURN
c$$$         ENDIF
c$$$         Z=VOUT(3)
c$$$         IF(Z.LE.ZIN(I)+TOLL.AND.Z.GE.ZIN(I)-TOLL) GOTO 100
c$$$         IF(Z.GT.ZIN(I)+TOLL) GOTO 10      
c$$$         IF(Z.LE.ZIN(I)-TOLL) THEN
c$$$            L = L - STEP
c$$$            STEP=STEP*(ZIN(I)-VECT(3))/(Z-VECT(3))
c$$$            DO J=1,7
c$$$               VECT(J)=VECTINI(J)
c$$$            ENDDO
c$$$            GOTO 11
c$$$         ENDIF
c$$$ 100     XOUT(I)=VOUT(1)
c$$$         YOUT(I)=VOUT(2)
c$$$         ZIN(I)=VOUT(3)
c$$$         IF(VOUT(3).ne.0)THEN
c$$$            THXOUT(I)=DATAN(VOUT(4)/VOUT(6))*180./ACOS(-1.)
c$$$            THYOUT(I)=DATAN(VOUT(5)/VOUT(6))*180./ACOS(-1.)
c$$$         ENDIF
c$$$         TLOUT(I) = L
c$$$c         print*,'D ',VECT(3),VOUT(3),VOUT(8),STEP
c$$$      ENDDO
c$$$
c$$$*     ==================================================================
c$$$*     track from reference plane UP
c$$$*     ==================================================================
c$$$*     parameters for GRKUTA:
c$$$*     -opposite charge
c$$$*     -opposite momentum direction
c$$$      IF(AL_P(5).NE.0) CHARGE=-AL_P(5)/DABS(AL_P(5))
c$$$      IF(AL_P(5).EQ.0) CHARGE=-1.
c$$$      VOUT(1)=AL_P(1)
c$$$      VOUT(2)=AL_P(2)
c$$$      VOUT(3)=ZINI             
c$$$      VOUT(4)=-AL_P(3)*DCOS(AL_P(4))
c$$$      VOUT(5)=-AL_P(3)*DSIN(AL_P(4))
c$$$      VOUT(6)=1.*DSQRT(1.-AL_P(3)**2)
c$$$      IF(AL_P(5).NE.0.) VOUT(7)=DABS(1./AL_P(5))
c$$$      IF(AL_P(5).EQ.0.) VOUT(7)=1.E8
c$$$      DO I=MIN((IUPDOWN-1),NPOINT),1,-1
c$$$         L = 0.0
c$$$
c$$$ 20      DO J=1,7
c$$$            VECT(J)=VOUT(J)
c$$$            VECTINI(J)=VOUT(J)
c$$$         ENDDO
c$$$         IF(VOUT(6).LE.0.) THEN
c$$$            IFAIL=1
c$$$c$$$            if(TRKVERBOSE)      
c$$$c$$$     $           PRINT *,'dofit (grkuta): WARNING ===> backward track!!'
c$$$            RETURN
c$$$         ENDIF
c$$$         step=(zin(i)-vect(3))/VOUT(6)
c$$$ 22      continue
c$$$         CALL GRKUTA(CHARGE,STEP,VECT,VOUT)
c$$$         L = L + STEP
c$$$         IF(VOUT(3).LT.VECT(3)) THEN
c$$$            IFAIL=1
c$$$c$$$            if(TRKVERBOSE)
c$$$c$$$     $      PRINT *,'dofit (grkuta): WARNING ===> backward track!!'
c$$$c$$$            if(TRKVERBOSE)print*,'charge',charge
c$$$c$$$            if(TRKVERBOSE)print*,'vect',vect
c$$$c$$$            if(TRKVERBOSE)print*,'vout',vout
c$$$c$$$            if(TRKVERBOSE)print*,'step',step
c$$$c$$$            if(TRKVERBOSE)print*,'DeltaB',DELTA0,DELTA1
c$$$            RETURN
c$$$         ENDIF
c$$$         Z=VOUT(3)
c$$$         IF(Z.LE.ZIN(I)+TOLL.AND.Z.GE.ZIN(I)-TOLL) GOTO 200
c$$$         IF(Z.LT.ZIN(I)-TOLL) GOTO 20      
c$$$         IF(Z.GE.ZIN(I)+TOLL) THEN
c$$$            L = L - STEP
c$$$            STEP=STEP*(ZIN(I)-VECT(3))/(Z-VECT(3))
c$$$            DO J=1,7
c$$$               VECT(J)=VECTINI(J)
c$$$            ENDDO
c$$$            GOTO 22
c$$$         ENDIF
c$$$ 200     XOUT(I)=VOUT(1)
c$$$         YOUT(I)=VOUT(2)
c$$$         ZIN(I)=VOUT(3)
c$$$         IF(VOUT(3).ne.0)THEN
c$$$            THXOUT(I)=DATAN(VOUT(4)/VOUT(6))*180./ACOS(-1.)
c$$$            THYOUT(I)=DATAN(VOUT(5)/VOUT(6))*180./ACOS(-1.)
c$$$         ENDIF
c$$$         TLOUT(I) = L
c$$$      ENDDO
c$$$
      RETURN
      END

************************************************************************
*
*
*
*
************************************************************************
      SUBROUTINE DOTRACK(NPOINT,ZIN,XOUT,YOUT,AL_P,IFAIL)

      IMPLICIT DOUBLE PRECISION (A-H,O-Z)

*     -----------------------------------------------
*     I/O parameters
      PARAMETER (NPOINT_MAX=500) ! EM it was =100
      DIMENSION ZIN(NPOINT_MAX)
      DIMENSION XOUT(NPOINT_MAX),YOUT(NPOINT_MAX)
      DIMENSION THXOUT(NPOINT_MAX),THYOUT(NPOINT_MAX)
      DIMENSION TLOUT(NPOINT_MAX)
      DIMENSION AL_P(5)
*     -----------------------------------------------      

      CALL DOTRACK2(NPOINT,ZIN,XOUT,YOUT,THXOUT,THYOUT,TLOUT,AL_P,IFAIL)

ccc
ccc   OLD-OLD-OLD-OLD-OLD-OLD-OLD-OLD-OLD-OLD-OLD-OLD-OLD-OLD-OLD-OLD-OLD-OLD
ccc
c$$$      DIMENSION VECT(7),VECTINI(7),VOUT(7)
c$$$      DATA TOLL/1.d-8/          
c$$$*     tolerance in reaching the next plane during the tracking procedure
c$$$*     -----------------------------------------------
c$$$*     I/O parameters
c$$$      PARAMETER (NPOINT_MAX=100)
c$$$      DIMENSION ZIN(NPOINT_MAX)
c$$$      DIMENSION XOUT(NPOINT_MAX),YOUT(NPOINT_MAX)
c$$$      DIMENSION AL_P(5)
c$$$*     -----------------------------------------------      
c$$$      DATA ZINI/23.5/           !z coordinate of the reference plane
c$$$      
c$$$*     ==================================================================
c$$$*     divide the track in two parts: below and above the reference plane
c$$$*     ==================================================================      
c$$$      IUPDOWN=0
c$$$      DO I=1,NPOINT   
c$$$         IF(ZIN(I).LT.ZINI)THEN
c$$$            if(i.ne.1)IUPDOWN=I
c$$$            GOTO 88
c$$$         ENDIF
c$$$         IUPDOWN=NPOINT+1
c$$$      ENDDO
c$$$ 88   CONTINUE
c$$$
c$$$*     ==================================================================
c$$$*     track from reference plane DOWN
c$$$*     ==================================================================
c$$$*     parameters for GRKUTA
c$$$      IF(AL_P(5).NE.0) CHARGE=AL_P(5)/DABS(AL_P(5))
c$$$      IF(AL_P(5).EQ.0) CHARGE=1.
c$$$      VOUT(1)=AL_P(1)
c$$$      VOUT(2)=AL_P(2)
c$$$      VOUT(3)=ZINI             
c$$$      VOUT(4)=AL_P(3)*DCOS(AL_P(4))
c$$$      VOUT(5)=AL_P(3)*DSIN(AL_P(4))
c$$$      VOUT(6)=-1.*DSQRT(1.-AL_P(3)**2)
c$$$      IF(AL_P(5).NE.0.) VOUT(7)=DABS(1./AL_P(5))
c$$$      IF(AL_P(5).EQ.0.) VOUT(7)=1.E8
c$$$      DO I=MAX(IUPDOWN,1),NPOINT 
c$$$         step=vout(3)-zin(i)
c$$$c$$$         print*,'DOWN ',i,' - Track from ',
c$$$c$$$     $        vout(3),' to ',zin(i),' step ',step
c$$$ 10      DO J=1,7
c$$$            VECT(J)=VOUT(J)
c$$$            VECTINI(J)=VOUT(J)
c$$$         ENDDO
c$$$ 11      continue
c$$$         CALL GRKUTA(CHARGE,STEP,VECT,VOUT)
c$$$         IF(VOUT(3).GT.VECT(3)) THEN
c$$$            IFAIL=1
c$$$c$$$            PRINT *,'=== WARNING ===> tracciamento invertito (DOWN)'
c$$$c$$$            print*,'charge',charge
c$$$c$$$            print*,'vect',vect
c$$$c$$$            print*,'vout',vout
c$$$c$$$            print*,'step',step
c$$$           RETURN
c$$$         ENDIF
c$$$         Z=VOUT(3)
c$$$         IF(Z.LE.ZIN(I)+TOLL.AND.Z.GE.ZIN(I)-TOLL) GOTO 100
c$$$         IF(Z.GT.ZIN(I)+TOLL) GOTO 10      
c$$$         IF(Z.LE.ZIN(I)-TOLL) THEN
c$$$            STEP=STEP*(ZIN(I)-VECT(3))/(Z-VECT(3))
c$$$            DO J=1,7
c$$$               VECT(J)=VECTINI(J)
c$$$            ENDDO
c$$$            GOTO 11
c$$$         ENDIF
c$$$ 100     XOUT(I)=VOUT(1)
c$$$         YOUT(I)=VOUT(2)
c$$$         ZIN(I)=VOUT(3)
c$$$*         THXOUT(I)=
c$$$*         THYOUT(I)=
c$$$      ENDDO
c$$$
c$$$
c$$$
c$$$*     ==================================================================
c$$$*     track from reference plane UP
c$$$*     ==================================================================
c$$$*     parameters for GRKUTA:
c$$$*     -opposite charge
c$$$*     -opposite momentum direction
c$$$      IF(AL_P(5).NE.0) CHARGE=-AL_P(5)/DABS(AL_P(5))
c$$$      IF(AL_P(5).EQ.0) CHARGE=-1.
c$$$      VOUT(1)=AL_P(1)
c$$$      VOUT(2)=AL_P(2)
c$$$      VOUT(3)=ZINI             
c$$$      VOUT(4)=-AL_P(3)*DCOS(AL_P(4))
c$$$      VOUT(5)=-AL_P(3)*DSIN(AL_P(4))
c$$$      VOUT(6)=1.*DSQRT(1.-AL_P(3)**2)
c$$$      IF(AL_P(5).NE.0.) VOUT(7)=DABS(1./AL_P(5))
c$$$      IF(AL_P(5).EQ.0.) VOUT(7)=1.E8
c$$$      DO I=MIN((IUPDOWN-1),NPOINT),1,-1
c$$$         step=vout(3)-zin(i)
c$$$         step = -step
c$$$c$$$         print*,'UP ',i,' - Track from ',
c$$$c$$$     $        vout(3),' to ',zin(i),' step ',step
c$$$ 20      DO J=1,7
c$$$            VECT(J)=VOUT(J)
c$$$            VECTINI(J)=VOUT(J)
c$$$         ENDDO
c$$$ 22      continue
c$$$         CALL GRKUTA(CHARGE,STEP,VECT,VOUT)
c$$$         IF(VOUT(3).LT.VECT(3)) THEN
c$$$            IFAIL=1
c$$$c$$$            PRINT *,'=== WARNING ===> tracciamento invertito (UP)'
c$$$c$$$            print*,'charge',charge
c$$$c$$$            print*,'vect',vect
c$$$c$$$            print*,'vout',vout
c$$$c$$$            print*,'step',step
c$$$            RETURN
c$$$         ENDIF
c$$$         Z=VOUT(3)
c$$$         IF(Z.LE.ZIN(I)+TOLL.AND.Z.GE.ZIN(I)-TOLL) GOTO 200
c$$$         IF(Z.LT.ZIN(I)-TOLL) GOTO 20      
c$$$         IF(Z.GE.ZIN(I)+TOLL) THEN
c$$$            STEP=STEP*(ZIN(I)-VECT(3))/(Z-VECT(3))
c$$$            DO J=1,7
c$$$               VECT(J)=VECTINI(J)
c$$$            ENDDO
c$$$            GOTO 22
c$$$         ENDIF
c$$$ 200     XOUT(I)=VOUT(1)
c$$$         YOUT(I)=VOUT(2)
c$$$         ZIN(I)=VOUT(3)
c$$$
c$$$      ENDDO

      RETURN
      END
