src/F77/mini.f

************************************************************************     
*
*     subroutine to evaluate the vector alfa (AL) 
*     which minimizes CHI^2
*     
*     - modified from mini.f in order to call differente chi^2 routine.
*     The new one includes also single clusters: in this case 
*     the residual is defined as the distance between the track and the 
*     segment AB associated to the single cluster.
*
*
************************************************************************  


      SUBROUTINE MINI2(ISTEP,IFAIL,IPRINT)
      
      IMPLICIT DOUBLE PRECISION (A-H,O-Z)

      include 'commontracker.f' !tracker general common
      include 'common_mini_2.f' !common for the tracking procedure

c      logical DEBUG
c      common/dbg/DEBUG

      parameter (dinf=1.d15)      !just a huge number...
c------------------------------------------------------------------------
c     variables used in the tracking procedure (mini and its subroutines)
c
c     N.B.: in mini & C. (and in the following block of variables too)
c     the plane ordering is reversed in respect of normal
c     ordering, but they maintain their Z coordinates. so plane number 1 is
c     the first one that a particle meets, and its Z coordinate is > 0
c------------------------------------------------------------------------
      DATA ZINI/23.5/  !!! ***PP*** to be changed !z coordinate of the reference plane

c      DATA XGOOD,YGOOD/nplanes*1.,nplanes*1./ !planes to be used in the tracking

      DATA STEPAL/5*1.d-7/      !alpha vector step
      DATA ISTEPMAX/100/        !maximum number of steps in the chi^2 minimization
      DATA TOLL/1.d-8/          !tolerance in reaching the next plane during 
*                               !the tracking procedure
      DATA STEPMAX/100./        !maximum number of steps in the trackin gprocess

c      DATA ALMAX/dinf,dinf,1.,dinf,dinf/ !limits on alpha vector components
c      DATA ALMIN/-dinf,-dinf,-1.,-dinf,-dinf/ !"
      DATA ALMAX/dinf,dinf,dinf,dinf,dinf/ !limits on alpha vector components
      DATA ALMIN/-dinf,-dinf,-dinf,-dinf,-dinf/ !"

      DIMENSION DAL(5)                    !increment of vector alfa
      DIMENSION CHI2DD_R(4,4),CHI2D_R(4) !hessiano e gradiente di chi2

c     elena--------
      REAL*8 AVRESX,AVRESY
c     elena--------

      INTEGER IFLAG             
c--------------------------------------------------------
c     IFLAG =1 ---- chi2 derivatives computed by using 
c                   incremental ratios and posxyz.f 
c     IFLAG =2 ---- the approximation of Golden is used
c                   (see chisq.f)
c
c     NB: the two metods gives equivalent results BUT 
c     method 2 is faster!!
c--------------------------------------------------------
      DATA IFLAG/2/   
         
c      LOGICAL TRKDEBUG,TRKVERBOSE
c      COMMON/TRKD/TRKDEBUG,TRKVERBOSE
      LOGICAL TRKDEBUG,TRKVERBOSE
      COMMON/TRKD/TRKDEBUG,TRKVERBOSE

      IF(IPRINT.EQ.1) THEN
         TRKVERBOSE = .TRUE.
         TRKDEBUG   = .FALSE.
      ELSEIF(IPRINT.EQ.2)THEN
         TRKVERBOSE = .TRUE.
         TRKDEBUG   = .TRUE.
      ELSE
         TRKVERBOSE = .FALSE.
         TRKDEBUG   = .FALSE.
      ENDIF         

*     ----------------------------------------------------------
*     evaluate average spatial resolution
*     ----------------------------------------------------------
      AVRESX = RESXAV
      AVRESY = RESYAV
      DO IP=1,6
         IF( XGOOD(IP).EQ.1 )THEN 
            NX=NX+1
            AVRESX=AVRESX+RESX(IP)
         ENDIF
         IF(NX.NE.0)AVRESX=AVRESX/NX
         IF( YGOOD(IP).EQ.1 )THEN 
            NY=NY+1
            AVRESY=AVRESY+RESY(IP)
         ENDIF
         IF(NX.NE.0)AVRESY=AVRESY/NY         
      ENDDO

*     ----------------------------------------------------------
*     define ALTOL(5) ---> tolerances on state vector
*     
*     ----------------------------------------------------------
*     changed in order to evaluate energy-dependent 
*     tolerances on all 5 parameters
      FACT=100.                  !scale factor to define tolerance on alfa
c     deflection error (see PDG)
      DELETA1 = 0.01/0.3/0.4/0.4451**2*SQRT(720./(6.+4.))
      DELETA2 = 0.016/0.3/0.4/0.4451*SQRT(0.4451/9.36)
c$$$      ALTOL(1) = AVRESX/FACT     !al(1) = x
c$$$      ALTOL(2) = AVRESY/FACT     !al(2) = y
c$$$      ALTOL(3) = DSQRT(AVRESX**2 !al(3)=sin(theta)
c$$$     $     +AVRESY**2)/44.51/FACT 
c$$$      ALTOL(4) = ALTOL(3)         !al(4)=phi
c     deflection error (see PDG)
c$$$      DELETA1 = 0.01*AVRESX/0.3/0.4/0.4451**2*SQRT(720./(6.+4.))
c$$$      DELETA2 = 0.016/0.3/0.4/0.4451*SQRT(0.4451/9.36)
*     ----------------------------------------------------------
*     
      ISTEP=0                   !num. steps to minimize chi^2
      JFAIL=0                   !error flag

      if(TRKDEBUG) print*,'guess: ',al
      if(TRKDEBUG) print*,'mini2: step ',istep,chi2,1./AL(5)

*     
*     -----------------------
*     START MINIMIZATION LOOP
*     -----------------------
 10   ISTEP=ISTEP+1             !<<<<<<<<<<<<<< NEW STEP !!

      CALL CHISQ(IFLAG,JFAIL)   !chi^2 and its derivatives
      IF(JFAIL.NE.0) THEN
         IFAIL=1
         CHI2=-9999.
         if(TRKVERBOSE) 
     $        PRINT *,'*** ERROR in mini *** wrong CHISQ'
         RETURN
      ENDIF
      
      COST=1e-7
      DO I=1,5
         DO J=1,5
            CHI2DD(I,J)=CHI2DD(I,J)*COST
         ENDDO
         CHI2D(I)=CHI2D(I)*COST
      ENDDO      

      IF(PFIXED.EQ.0.) THEN

*------------------------------------------------------------*
*     track fitting with FREE deflection
*------------------------------------------------------------*
         CALL DSFACT(5,CHI2DD,5,IFA,DET,JFA) !CHI2DD matrix determinant
         IF(IFA.NE.0) THEN      !not positive-defined      
            if(TRKVERBOSE)then
               PRINT *,
     $              '*** ERROR in mini ***'//
     $              'on matrix inversion (not pos-def)'
     $              ,DET
            endif
            IF(CHI2.EQ.0) CHI2=-9999.
            IF(CHI2.GT.0) CHI2=-CHI2
            IFAIL=1
            RETURN             
         ENDIF
         CALL DSFINV(5,CHI2DD,5) !CHI2DD matrix inversion    
*     *******************************************
*     find new value of AL-pha                 
*     *******************************************                                        
         DO I=1,5                              
            DAL(I)=0.           
            DO J=1,5            
               DAL(I)=DAL(I)-CHI2DD(I,J)*CHI2D(J) 
               COV(I,J)=2.*COST*CHI2DD(I,J) 
            ENDDO               
         ENDDO                  
         DO I=1,5               
            AL(I)=AL(I)+DAL(I)  
         ENDDO                  
*------------------------------------------------------------*
*     track fitting with FIXED deflection
*------------------------------------------------------------*
      ELSE
         AL(5)=1./PFIXED
         DO I=1,4
            CHI2D_R(I)=CHI2D(I)
            DO J=1,4
               CHI2DD_R(I,J)=CHI2DD(I,J)
            ENDDO
         ENDDO
         CALL DSFACT(4,CHI2DD_R,4,IFA,DET,JFA)
         IF(IFA.NE.0) THEN
            if(TRKVERBOSE)then
               PRINT *,
     $              '*** ERROR in mini ***'//
     $              'on matrix inversion (not pos-def)'
     $              ,DET
            endif
            IF(CHI2.EQ.0) CHI2=-9999.
            IF(CHI2.GT.0) CHI2=-CHI2
            IFAIL=1
            RETURN             
         ENDIF
         CALL DSFINV(4,CHI2DD_R,4)
*     *******************************************
*     find new value of AL-pha                 
*     *******************************************                                        
         DO I=1,4
            DAL(I)=0.
            DO J=1,4
               DAL(I)=DAL(I)-CHI2DD_R(I,J)*CHI2D_R(J)
               COV(I,J)=2.*COST*CHI2DD_R(I,J)
            ENDDO
         ENDDO
         DAL(5)=0.
         DO I=1,4
            AL(I)=AL(I)+DAL(I)
         ENDDO
      ENDIF

      if(TRKDEBUG) print*,'mini2: step ',istep,chi2,1./AL(5)

*------------------------------------------------------------*
*     ----------------------------------------------------   *
*------------------------------------------------------------*
*     check parameter bounds:
*------------------------------------------------------------*
      DO I=1,5
         IF(AL(I).GT.ALMAX(I).OR.AL(I).LT.ALMIN(I))THEN
            if(TRKVERBOSE)then
               PRINT*,' *** WARNING in mini ***  '
               PRINT*,'MINI_2 ==> AL(',I,') out of range'
               PRINT*,'         value: ',AL(I),
     $              '  limits: ',ALMIN(I),ALMAX(I)               
               print*,'istep ',istep
            endif
            IF(CHI2.EQ.0) CHI2=-9999.
            IF(CHI2.GT.0) CHI2=-CHI2
            IFAIL=1
            RETURN
         ENDIF
      ENDDO      
*------------------------------------------------------------*
*     check number of steps:
*------------------------------------------------------------*
      IF(ISTEP.ge.ISTEPMAX) then
c$$$         IFAIL=1
c$$$         if(TRKVERBOSE)
c$$$     $        PRINT *,'*** WARNING in mini *** ISTEP.GT.ISTEPMAX=',
c$$$     $        ISTEPMAX
         goto 11
      endif
*------------------------------------------------------------*
*     ---------------------------------------------
*     evaluate deflection tolerance on the basis of 
*     estimated deflection
*     ---------------------------------------------
*------------------------------------------------------------*
c$$$      ALTOL(5) = DSQRT(DELETA1**2+DELETA2**2*AL(5)**2)/FACT
      ALTOL(5) = DSQRT((DELETA1*AVRESX)**2+DELETA2**2*AL(5)**2)/FACT
      ALTOL(1) = ALTOL(5)/DELETA1
      ALTOL(2) = ALTOL(1)
      ALTOL(3) = DSQRT(ALTOL(1)**2+ALTOL(2)**2)/44.51
      ALTOL(4) = ALTOL(3)        
      
*---- check tolerances: 
c$$$      DO I=1,5
c$$$         if(TRKVERBOSE)print*,i,' -- ',DAL(I),ALTOL(I) !>>>> new step!
c$$$      ENDDO
c$$$      print*,'chi2 -- ',DCHI2

      DO I=1,5
         IF(ABS(DAL(I)).GT.ALTOL(I))GOTO 10 !>>>> new step!
      ENDDO

*     new estimate of chi^2:
      JFAIL=0                   !error flag
      CALL CHISQ(IFLAG,JFAIL)   !chi^2 and its derivatives
      IF(JFAIL.NE.0) THEN
         IFAIL=1
         if(TRKVERBOSE)THEN
            CHI2=-9999.
         if(TRKVERBOSE) 
     $        PRINT *,'*** ERROR in mini *** wrong CHISQ'
         ENDIF
         RETURN
      ENDIF
      COST=1e-7
      DO I=1,5
         DO J=1,5
            CHI2DD(I,J)=CHI2DD(I,J)*COST
         ENDDO
         CHI2D(I)=CHI2D(I)*COST
      ENDDO      
      IF(PFIXED.EQ.0.) THEN
         CALL DSFACT(5,CHI2DD,5,IFA,DET,JFA) !CHI2DD matrix determinant
         IF(IFA.NE.0) THEN      !not positive-defined      
            if(TRKVERBOSE)then
               PRINT *,
     $              '*** ERROR in mini ***'//
     $              'on matrix inversion (not pos-def)'
     $              ,DET
            endif
            IF(CHI2.EQ.0) CHI2=-9999.
            IF(CHI2.GT.0) CHI2=-CHI2
            IFAIL=1
            RETURN             
         ENDIF
         CALL DSFINV(5,CHI2DD,5) !CHI2DD matrix inversion    
         DO I=1,5                              
            DAL(I)=0.           
            DO J=1,5            
               COV(I,J)=2.*COST*CHI2DD(I,J) 
            ENDDO               
         ENDDO                  
      ELSE
         DO I=1,4
            CHI2D_R(I)=CHI2D(I)
            DO J=1,4
               CHI2DD_R(I,J)=CHI2DD(I,J)
            ENDDO
         ENDDO
         CALL DSFACT(4,CHI2DD_R,4,IFA,DET,JFA)
         IF(IFA.NE.0) THEN
            if(TRKVERBOSE)then
               PRINT *,
     $              '*** ERROR in mini ***'//
     $              'on matrix inversion (not pos-def)'
     $              ,DET
            endif
            IF(CHI2.EQ.0) CHI2=-9999.
            IF(CHI2.GT.0) CHI2=-CHI2
            IFAIL=1
            RETURN             
         ENDIF
         CALL DSFINV(4,CHI2DD_R,4)
         DO I=1,4
            DAL(I)=0.
            DO J=1,4
               COV(I,J)=2.*COST*CHI2DD_R(I,J)
            ENDDO
         ENDDO
      ENDIF
*****************************

*     ------------------------------------
*     Number of Degree Of Freedom
      ndof=0                                
      do ip=1,nplanes
         ndof=ndof
     $        +int(xgood(ip))
     $        +int(ygood(ip))
      enddo
      if(pfixed.eq.0.) ndof=ndof-5 ! ***PP***                          
      if(pfixed.ne.0.) ndof=ndof-4 ! ***PP***
      if(ndof.le.0.) then
         ndof = 1
         if(TRKVERBOSE)
     $        print*,'*** WARNING *** in mini n.dof = 0 (set to 1)' 
      endif

      if(TRKDEBUG) print*,'mini2: -ok- ',istep,chi2,1./AL(5)

*     ------------------------------------
*     Reduced chi^2
      CHI2 = CHI2/dble(ndof)

c      print*,'mini2: chi2 ',chi2

 11   CONTINUE       

      NSTEP=ISTEP ! ***PP***

      RETURN
      END         

******************************************************************************     
*     
*     routine to compute chi^2 and its derivatives
*     
*
*     (modified in respect to the previous one in order to include
*     single clusters. In this case the residual is evaluated by 
*     calculating the distance between the track intersection and the 
*     segment AB associated to the single cluster)
*
******************************************************************************

      SUBROUTINE CHISQ(IFLAG,IFAIL)

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

      include 'commontracker.f' !tracker general common
      include 'common_mini_2.f' !common for the tracking procedure

      DIMENSION XV2(nplanes),YV2(nplanes),XV1(nplanes),YV1(nplanes)
     $     ,XV0(nplanes),YV0(nplanes)
      DIMENSION AL_P(5)

c      LOGICAL TRKVERBOSE
c      COMMON/TRKD/TRKVERBOSE
      LOGICAL TRKDEBUG,TRKVERBOSE
      COMMON/TRKD/TRKDEBUG,TRKVERBOSE
*     
*     chi^2 computation
*     
      DO I=1,5
         AL_P(I)=AL(I)
      ENDDO
      JFAIL=0                   !error flag
      CALL POSXYZ(AL_P,JFAIL)   !track intersection with tracking planes
      IF(JFAIL.NE.0) THEN
         IF(TRKVERBOSE)
     $        PRINT *,'CHISQ ==> error from trk routine POSXYZ !!'
         IFAIL=1
         RETURN
      ENDIF
      DO I=1,nplanes
         XV0(I)=XV(I)
         YV0(I)=YV(I)
      ENDDO
*     ------------------------------------------------
c$$$      CHI2=0.
c$$$      DO I=1,nplanes
c$$$         CHI2=CHI2
c$$$     +        +(XV(I)-XM(I))**2/RESX(i)**2   *XGOOD(I)*YGOOD(I)
c$$$     +        +(YV(I)-YM(I))**2/RESY(i)**2   *YGOOD(I)*XGOOD(I)
c$$$      ENDDO
*     ---------------------------------------------------------
*     For planes with only a X or Y-cl included, instead of 
*     a X-Y couple, the residual for chi^2 calculation is
*     evaluated by finding the point x-y, along the segment AB,
*     closest to the track.
*     The X or Y coordinate, respectivelly for X and Y-cl, is
*     then assigned to XM or YM, which is then considered the 
*     measured position of the cluster.
*     ---------------------------------------------------------
      CHI2=0.
      DO I=1,nplanes
         IF(XGOOD(I).EQ.1.AND.YGOOD(I).EQ.0)THEN !X-cl
            BETA = (XM_B(I)-XM_A(I))/(YM_B(I)-YM_A(I))
            ALFA = XM_A(I) - BETA * YM_A(I)
            YM(I) = ( YV(I) + BETA*XV(I) - BETA*ALFA )/(1+BETA**2)
            if(YM(I).lt.dmin1(YM_A(I),YM_B(I)))
     $           YM(I)=dmin1(YM_A(I),YM_B(I))
            if(YM(I).gt.dmax1(YM_A(I),YM_B(I)))
     $           YM(I)=dmax1(YM_A(I),YM_B(I))
            XM(I) = ALFA + BETA * YM(I) !<<<< measured coordinates
         ELSEIF(XGOOD(I).EQ.0.AND.YGOOD(I).EQ.1)THEN !Y-cl
            BETA = (YM_B(I)-YM_A(I))/(XM_B(I)-XM_A(I))
            ALFA = YM_A(I) - BETA * XM_A(I)
            XM(I) = ( XV(I) + BETA*YV(I) - BETA*ALFA )/(1+BETA**2)
            if(XM(I).lt.dmin1(XM_A(I),XM_B(I)))
     $           XM(I)=dmin1(XM_A(I),XM_B(I))
            if(XM(I).gt.dmax1(XM_A(I),XM_B(I)))
     $           XM(I)=dmax1(XM_A(I),XM_B(I))
            YM(I) = ALFA + BETA * XM(I) !<<<< measured coordinates
         ENDIF
         CHI2=CHI2
     +        +(XV(I)-XM(I))**2/RESX(i)**2   *( XGOOD(I)*YGOOD(I) )
     +        +(YV(I)-YM(I))**2/RESY(i)**2   *( YGOOD(I)*XGOOD(I) )
     +        +((XV(I)-XM(I))**2+(YV(I)-YM(I))**2)/RESX(i)**2 
     +                                       *( XGOOD(I)*(1-YGOOD(I)) )
     +        +((XV(I)-XM(I))**2+(YV(I)-YM(I))**2)/RESY(i)**2 
     +                                       *( (1-XGOOD(I))*YGOOD(I) )
      ENDDO
c      print*,'CHISQ ',chi2
*     ------------------------------------------------
*     
*     calculation of derivatives (dX/dAL_fa and dY/dAL_fa)     
*
*     //////////////////////////////////////////////////
*     METHOD 1 -- incremental ratios
*     //////////////////////////////////////////////////

      IF(IFLAG.EQ.1) THEN
        
         DO J=1,5
            DO JJ=1,5
               AL_P(JJ)=AL(JJ)
            ENDDO
            AL_P(J)=AL_P(J)+STEPAL(J)/2.
            JFAIL=0
            CALL POSXYZ(AL_P,JFAIL)
            IF(JFAIL.NE.0) THEN
               IF(TRKVERBOSE)
*23456789012345678901234567890123456789012345678901234567890123456789012
     $              PRINT *,'CHISQ ==> error from trk routine POSXYZ'
               IFAIL=1
               RETURN
            ENDIF
            DO I=1,nplanes
               XV2(I)=XV(I)
               YV2(I)=YV(I)
            ENDDO
            AL_P(J)=AL_P(J)-STEPAL(J)
            JFAIL=0
            CALL POSXYZ(AL_P,JFAIL)
            IF(JFAIL.NE.0) THEN
               IF(TRKVERBOSE)
     $              PRINT *,'CHISQ ==> error from trk routine POSXYZ'
               IFAIL=1
               RETURN
            ENDIF
            DO I=1,nplanes
               XV1(I)=XV(I)
               YV1(I)=YV(I)
            ENDDO
            DO I=1,nplanes
               DXDAL(I,J)=(XV2(I)-XV1(I))/STEPAL(J)
               DYDAL(I,J)=(YV2(I)-YV1(I))/STEPAL(J)
            ENDDO
         ENDDO
         
      ENDIF
      
*     //////////////////////////////////////////////////
*     METHOD 2 -- Bob Golden
*     //////////////////////////////////////////////////

      IF(IFLAG.EQ.2) THEN
        
         DO I=1,nplanes
            DXDAL(I,1)=1.
            DYDAL(I,1)=0.
            
            DXDAL(I,2)=0.
            DYDAL(I,2)=1.
            
            COSTHE=DSQRT(1.-AL(3)**2)
            IF(COSTHE.EQ.0.) THEN
               IF(TRKVERBOSE)PRINT *,'=== WARNING ===> COSTHE=0'
               IFAIL=1
               RETURN
            ENDIF
            
            DXDAL(I,3)=(ZINI-ZM(I))*DCOS(AL(4))/COSTHE**3
            DYDAL(I,3)=(ZINI-ZM(I))*DSIN(AL(4))/COSTHE**3
            
            DXDAL(I,4)=-AL(3)*(ZINI-ZM(I))*DSIN(AL(4))/COSTHE
            DYDAL(I,4)=AL(3)*(ZINI-ZM(I))*DCOS(AL(4))/COSTHE
            
            IF(AL(5).NE.0.) THEN      
               DXDAL(I,5)=
     +         (XV(I)-(AL(1)+AL(3)/COSTHE*(ZINI-ZM(I))
     +         *DCOS(AL(4))))/AL(5)
               DYDAL(I,5)=
     +         (YV(I)-(AL(2)+AL(3)/COSTHE*(ZINI-ZM(I))
     +         *DSIN(AL(4))))/AL(5)
            ELSE
               DXDAL(I,5)=100.*( 0.25 *0.3*0.4*(0.01*(ZINI-ZM(I)))**2 )
               DYDAL(I,5)=0.
            ENDIF
            
         ENDDO
      ENDIF
*
*     x-x-x-x-x-x-x-x-x-x-x-x-x-x-x-x-x-x-x-x-x-x-x-x-x-x-x-x-x-x-x-x-x    
*     >>> CHI2D evaluation
*     
      DO J=1,5
         CHI2D(J)=0.      
         DO I=1,nplanes
            CHI2D(J)=CHI2D(J)
     +           +2.*(XV0(I)-XM(I))/RESX(i)**2*DXDAL(I,J)   *XGOOD(I)
     +           +2.*(YV0(I)-YM(I))/RESY(i)**2*DYDAL(I,J)   *YGOOD(I)
         ENDDO
      ENDDO
*     
*     >>> CHI2DD evaluation
*     
      DO I=1,5
         DO J=1,5
            CHI2DD(I,J)=0.
            DO K=1,nplanes
               CHI2DD(I,J)=CHI2DD(I,J)
     +              +2.*DXDAL(K,I)*DXDAL(K,J)/RESX(k)**2   *XGOOD(K)
     +              +2.*DYDAL(K,I)*DYDAL(K,J)/RESY(k)**2   *YGOOD(K)
            ENDDO
         ENDDO
      ENDDO     
*     x-x-x-x-x-x-x-x-x-x-x-x-x-x-x-x-x-x-x-x-x-x-x-x-x-x-x-x-x-x-x-x-x

      RETURN
      END


*****************************************************************
*
*     Routine to compute the track intersection points
*     on the tracking-system planes, 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 equatins of motion with Runge-Kuta method.
*
*     Variables that have to be assigned when the subroutine 
*     is called are:
*
*     ZM(1,NPLANES) ----> 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 starting from the
*     reference plane (ZINI)
*     -----------------------------------------------------------
*     
*****************************************************************

      SUBROUTINE POSXYZ(AL_P,IFAIL)

      IMPLICIT DOUBLE PRECISION (A-H,O-Z)
      
      include 'commontracker.f' !tracker general common
      include 'common_mini_2.f' !common for the tracking procedure

c      LOGICAL TRKVERBOSE
c      COMMON/TRKD/TRKVERBOSE
      LOGICAL TRKDEBUG,TRKVERBOSE
      COMMON/TRKD/TRKDEBUG,TRKVERBOSE
c      
      DIMENSION AL_P(5)
*     
      DO I=1,nplanes
         ZV(I)=ZM(I)            !
      ENDDO      
*     
*     set 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              !  DBLE(Z0)-DBLE(ZSPEC)
      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

c$$$      print*,'POSXY ',vout

      DO I=1,nplanes
         step=vout(3)-zv(i)
 10      DO J=1,7
            VECT(J)=VOUT(J)
            VECTINI(J)=VOUT(J)
         ENDDO
 11      continue
         CALL GRKUTA(CHARGE,STEP,VECT,VOUT)
         IF(VOUT(3).GT.VECT(3)) THEN
            IFAIL=1
            if(TRKVERBOSE)
     $      PRINT *,'posxy (grkuta): WARNING ===> backward track!!'
c$$$            if(.TRUE.)print*,'charge',charge
c$$$            if(.TRUE.)print*,'vect',vect
c$$$            if(.TRUE.)print*,'vout',vout
c$$$            if(.TRUE.)print*,'step',step
            if(TRKVERBOSE)print*,'charge',charge
            if(TRKVERBOSE)print*,'vect',vect
            if(TRKVERBOSE)print*,'vout',vout
            if(TRKVERBOSE)print*,'step',step
            RETURN
         ENDIF
         Z=VOUT(3)
         IF(Z.LE.ZM(I)+TOLL.AND.Z.GE.ZM(I)-TOLL) GOTO 100
         IF(Z.GT.ZM(I)+TOLL) GOTO 10      
         IF(Z.LE.ZM(I)-TOLL) THEN
            STEP=STEP*(ZM(I)-VECT(3))/(Z-VECT(3))
            DO J=1,7
               VECT(J)=VECTINI(J)
            ENDDO
            GOTO 11
         ENDIF

*     -----------------------------------------------
*        evaluate track coordinates
 100     XV(I)=VOUT(1)
         YV(I)=VOUT(2)
         ZV(I)=VOUT(3)
         AXV(I)=DATAN(VOUT(4)/VOUT(6))*180./ACOS(-1.)
         AYV(I)=DATAN(VOUT(5)/VOUT(6))*180./ACOS(-1.)
*     -----------------------------------------------

      ENDDO

      RETURN
      END


*     **********************************************************
*     Some initialization routines
*     **********************************************************

*     ----------------------------------------------------------
*     Routine to initialize COMMON/TRACK/
*
      subroutine track_init
      
      IMPLICIT DOUBLE PRECISION (A-H,O-Z)

      include 'commontracker.f' !tracker general common
      include 'common_mini_2.f' !common for the tracking procedure
      include 'common_mech.f'

      do i=1,5
         AL(i) = 0.        
      enddo
      
      do ip=1,NPLANES
         ZM(IP) = fitz(nplanes-ip+1) !init to mech. position
         XM(IP) = -100.         !0.
         YM(IP) = -100.         !0.
         XM_A(IP) = -100.         !0.
         YM_A(IP) = -100.         !0.
c         ZM_A(IP) = 0
         XM_B(IP) =  -100.         !0.
         YM_B(IP) =  -100.         !0.
c         ZM_B(IP) = 0
         RESX(IP) = 1000.       !3.d-4
         RESY(IP) = 1000.       !12.d-4
         XGOOD(IP) = 0
         YGOOD(IP) = 0
      enddo

      return
      end 


***************************************************
*                                                 *
*                                                 *
*                                                 *
*                                                 *
*                                                 *
*                                                 *
**************************************************

      subroutine guess()

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

      include 'commontracker.f' !tracker general common
      include 'common_mini_2.f' !common for the tracking procedure
      
      REAL*4 XP(NPLANES),ZP(NPLANES),AP(NPLANES),RP(NPLANES)
      REAL*4 CHI,XC,ZC,RADIUS
*     ----------------------------------------
*     Y view
*     ----------------------------------------
*     ----------------------------------------
*     initial guess with a straigth line
*     ----------------------------------------
      SZZ=0.                    
      SZY=0.
      SSY=0.
      SZ=0.
      S1=0.
      DO I=1,nplanes
         IF(YGOOD(I).EQ.1)THEN
            YY = YM(I)
            IF(XGOOD(I).EQ.0)THEN
               YY = (YM_A(I) + YM_B(I))/2
            ENDIF
            SZZ=SZZ+ZM(I)*ZM(I)
            SZY=SZY+ZM(I)*YY
            SSY=SSY+YY
            SZ=SZ+ZM(I)
            S1=S1+1.
         ENDIF
      ENDDO
      DET=SZZ*S1-SZ*SZ
      AY=(SZY*S1-SZ*SSY)/DET
      BY=(SZZ*SSY-SZY*SZ)/DET
      Y0 = AY*ZINI+BY
*     ----------------------------------------
*     X view
*     ----------------------------------------
*     ----------------------------------------
*     1) initial guess with a circle
*     ----------------------------------------
      NP=0
      DO I=1,nplanes
         IF(XGOOD(I).EQ.1)THEN
            XX = XM(I)
            IF(YGOOD(I).EQ.0)THEN
               XX = (XM_A(I) + XM_B(I))/2
            ENDIF
            NP=NP+1
            XP(NP)=XX
            ZP(NP)=ZM(I)
         ENDIF
      ENDDO
      CALL TRICIRCLE(NP,XP,ZP,AP,RP,CHI,XC,ZC,RADIUS,IFLAG)
      print*,' circle: ',XC,ZC,RADIUS,' --- ',CHI,IFLAG
      IF(IFLAG.NE.0)GOTO 10 !straigth fit
      if(CHI.gt.100)GOTO 10 !straigth fit
      ARG = RADIUS**2-(ZINI-ZC)**2
      IF(ARG.LT.0)GOTO 10       !straigth fit
      DC = SQRT(ARG)      
      IF(XC.GT.0)DC=-DC
      X0=XC+DC
      AX = -(ZINI-ZC)/DC
      DEF=100./(RADIUS*0.3*0.43)
      IF(XC.GT.0)DEF=-DEF
      
      IF(ABS(X0).GT.30)THEN
         PRINT*,'STRANGE GUESS: XC,ZC,R ',XC,ZC,RADIUS
     $     ,' - CHI ',CHI,' - X0,AX,DEF ',X0,AX,DEF
         GOTO 10       !straigth fit
      ENDIF
      GOTO 20                   !guess is ok

*     ----------------------------------------
*     2) initial guess with a straigth line
*     - if circle does not intersect reference plane
*     - if bad chi**2
*     ----------------------------------------
 10   CONTINUE
      SZZ=0.                   
      SZX=0.
      SSX=0.
      SZ=0.
      S1=0.
      DO I=1,nplanes
         IF(XGOOD(I).EQ.1)THEN
            XX = XM(I)
            IF(YGOOD(I).EQ.0)THEN
               XX = (XM_A(I) + XM_B(I))/2
            ENDIF
            SZZ=SZZ+ZM(I)*ZM(I)
            SZX=SZX+ZM(I)*XX
            SSX=SSX+XX
            SZ=SZ+ZM(I)
            S1=S1+1.
         ENDIF
      ENDDO
      DET=SZZ*S1-SZ*SZ
      AX=(SZX*S1-SZ*SSX)/DET
      BX=(SZZ*SSX-SZX*SZ)/DET
      DEF = 0
      X0  = AX*ZINI+BX

 20   CONTINUE
*     ----------------------------------------
*     guess
*     ----------------------------------------

      AL(1) = X0
      AL(2) = Y0
      tath  = sqrt(AY**2+AX**2)
      AL(3) = tath/sqrt(1+tath**2)
      IF(AX.NE.0)THEN 
         AL(4)= atan(AY/AX)
      ELSE
         AL(4) = acos(-1.)/2 
         IF(AY.LT.0)AL(4) = AL(4)+acos(-1.)
      ENDIF
      IF(AX.LT.0)AL(4)= acos(-1.)+ AL(4)
      AL(4) = -acos(-1.) + AL(4) !from incidence direction to tracking rs
      AL(5) = DEF

c      print*,' guess: ',(al(i),i=1,5)

      end