!-----------------------------------------------------------------------
! track likelihood: calculated using hits in magnet chambers
! M. Staric, sept-2001
!
! SUBROUTINE TRACK_LIKEHOOD - mother routine
! SUBROUTINE TRACK_LKHD(IROW,PLKH,OK)
! SUBROUTINE MAGN_RESIDU(I0,XDIF)
! SUBROUTINE KOREKCIJA(ITIP,Z,PF,DX,SIG)
! SUBROUTINE MAGN_LIKEH(XWIN,XDIF,PLKH)
! SUBROUTINE GEDE_CLIP(J0,IROW,XDIF,IR1,IRL,NPASS)
! SUBROUTINE INTP_TRACK(Z,J0,C_ALFA,S_ALFA)
! SUBROUTINE KOVAR_MATRIKA(ISUP,K0,COV,FM)
! SUBROUTINE SIPALNI_DEL(TX,TY,PF,DZ,RADT,QMS)
! SUBROUTINE GET_MCSUPLAY
! SUBROUTINE SKOZI_MAGNET(X,Y,Z,TX,TY,PF,FRKD,Z1,IER)
!
!-----------------------------------------------------------------------

      SUBROUTINE TRACK_LIKEHOOD

      IMPLICIT NONE

#include "riter/FAKE_SUPL.fi"
#include "riter/FAKE_HITS.fi"
#include "arte/Arte.fi"
#include "auto/RTRA.fi"
#include "auto/RTRE.fi"
      INTEGER IROW
      REAL PLKH
      LOGICAL OK
#include "arte/Arte.ff"
#include "auto/RTRA.ff"
#include "auto/RTRE.ff"

      CALL GET_MCSUPLAY
      DO IROW=1,MIN(NRTRA(),NRTRE())
         Q(QRTRE_LMGT(IROW))=0.
         IQ(IQRTRE_FLMG(IROW))=0
         OK=IRTRA_FIT(IROW).EQ.0
         OK=OK.AND.IRTRA_HSI(IROW).GT.0
         OK=OK.AND.(IRTRA_HOU(IROW)+IRTRA_HIN(IROW)).GT.0
         IF(OK) THEN
            CALL TRACK_LKHD(IROW,PLKH,OK)
            IF(OK) THEN
               Q(QRTRE_LMGT(IROW))=PLKH
               IQ(IQRTRE_FLMG(IROW))=NPASS
            ENDIF
         ENDIF
      ENDDO     

      RETURN
      END



      SUBROUTINE TRACK_LKHD(IROW,PLKH,OK)

! returns track likelihood for given RTRA, if OK=.true.

      IMPLICIT NONE
      INTEGER IROW              ! RTRA row number
      REAL PLKH                 ! likelihood that track is real
      LOGICAL OK

#include "riter/FAKE_SUPL.fi"
#include "riter/FAKE_HITS.fi"
#include "arte/Arte.fi"
#include "auto/RTRA.fi"
      INTEGER I, J, K, KK, IER
      REAL FM(5,5), COV(5,5)
      LOGICAL SEKA1, SEKA2
      REAL XDIF, XWIN
      DATA XDIF /5./, XWIN /2./
      SAVE XDIF, XWIN
#include "arte/Arte.ff"
#include "auto/RTRA.ff"

      PLKH=0.
      OK=.FALSE.
! get RTRA
      ZP=FRTRA_ZF(IROW)
      XP=FRTRA_XF(IROW)
      YP=FRTRA_YF(IROW)
      TXP=FRTRA_TXF(IROW)
      TYP=FRTRA_TYF(IROW)
      PF=FRTRA_PF(IROW)
      KK=0
      DO K=1,5
         DO I=1,K
            KK=KK+1
            COV(I,K)=FRTRA_CVF(KK,IROW)
            IF(I.NE.K) COV(K,I)=COV(I,K)
         ENDDO
      ENDDO
! propagate thru magnet
      DO I=1,NSUPL
         J=IND_SUPL(I)
         DO K=1,3
            CALL SKOZI_MAGNET(XP,YP,ZP,TXP,TYP,PF,FM,Z_SUPL(K,J),IER)
            IF(IER.NE.0) RETURN
            X_SUPL(K,J)=XP
            Y_SUPL(K,J)=YP
            TX_SUPL(K,J)=TXP
            TY_SUPL(K,J)=TYP
            SEKA1=ABS(XP).LT.X0_SUPL(J).AND.ABS(YP).LT.Y0_SUPL(J)
            SEKA2=ABS(XP).LT.XR_SUPL(J).AND.ABS(YP).LT.YR_SUPL(J)
            SEKA_SUPL(K,J)=SEKA1.AND..NOT.SEKA2
            CALL KOVAR_MATRIKA(J,K,COV,FM)
         ENDDO
      ENDDO
      ROTA=-10.
! residuals
      NPASS=0
      NT=0
      DO I=1,NSUPL
         IF(Z_SUPL(1,I).GT.350) THEN
            CALL MAGN_RESIDU(I,XDIF)
         ENDIF
      ENDDO
      IF(NPASS.EQ.0) RETURN
! likelihood calculation
      CALL MAGN_LIKEH(XWIN,XDIF,PLKH)
      OK=.TRUE.

      RETURN
      END


      SUBROUTINE MAGN_RESIDU(I0,XDIF)

! collect magnet hits, calculate residuals and store them in /FAKE_HITS/

      IMPLICIT NONE
      
      INTEGER I0
      REAL XDIF

#include "riter/FAKE_SUPL.fi"
#include "riter/FAKE_HITS.fi"
#include "arte/Arte.fi"
#include "auto/GESL.fi"
#include "auto/GEDE.fi"
#include "auto/HITB.fi"
      INTEGER IROW, IR, IX, IR1, IRL, K
      INTEGER ITIP, ILAY, ISEC
      REAL DX, ZH, XH, SIGMA
      INTEGER IER1
      SAVE IER1
      REAL SIG_CELL(0:2) 
      DATA SIG_CELL /0., .1644, .3075/
      SAVE SIG_CELL
#include "arte/Arte.ff"
#include "auto/GESL.ff"
#include "auto/GEDE.ff"
#include "auto/HITB.ff"

      IF(NT.GE.MAXNT) RETURN
      IROW=GESL_ROW(I0)
      if(igesl_cmp(irow).eq.2.and.ifix(z_supl(2,i0)/50.).eq.7) return
      IX=PGESL_GEDE(IROW)
      DO WHILE(IX.GT.0)
         IR=MGESL_GEDE(IX)
         IX=XGESL_GEDE(IX)
         ILAY=IGEDE_LAY(IR)
         ISEC=IGEDE_SEC(IR)
         ITIP=0
         IF(IGEDE_CMP(IR).EQ.2) ITIP=(ISEC-1)/6+1
         CALL GEDE_CLIP(I0,IR,XDIF,IR1,IRL,NPASS)
         DO K=IR1,IRL
            ZH=FHITB_Z(K)
            XH=XP+TXP*(ZH-ZP)
            DX=FHITB_X(K)-XH
            IF(ABS(DX).LT.XDIF) THEN
               NT=NT+1
               IF(NT.GT.MAXNT) THEN
                  CALL MEPRIC(IER1,10,'RITER: magn_residu_: NT > MAXNT')
                  NT=MAXNT
                  RETURN
               ENDIF
               SIGMA=SQRT(FHITB_R(K)**2+C11+SIG_CELL(ITIP)**2)
               CALL KOREKCIJA(ITIP,ZH,PF,DX,SIGMA)
               DX_HIT(NT)=DX
               SIG_HIT(NT)=SIGMA
               Z_HIT(NT)=ZH
               HITB_ROW(NT)=K
               ITIP_HIT(NT)=ITIP
            ENDIF
         ENDDO
      ENDDO
      
      RETURN
      END



      SUBROUTINE KOREKCIJA(ITIP,Z,PF,DX,SIG)

! correction for residuals and sigmas - alignment dependent
! >> will be changed - have to be more general << 
! >> for keytable 16, based on run 16784 <<

      IMPLICIT NONE

      INTEGER ITIP
      REAL Z, PF
      REAL DX, SIG

      REAL XOFFST(4,0:2)
      DATA XOFFST /
     &     0., 0.07,  0.1, 0.,
     &     0., 0.09,  0.,   0.3,
     &     0., 0.13,  0.25, 0./
      SAVE XOFFST
      REAL SLOPE(4,0:2)
      DATA SLOPE /
     &     0., -1., -1., 0.,
     &     0., 0., 2., 1.,
     &     0., 0., 2., 0./
      SAVE SLOPE
      REAL PULL(4,0:2)
      DATA PULL /
     &     1., 2.12, 1.41, 1.,
     &     1., 1.18, 1.23, 1.22,
     &     1., 1.22, 1.35, 1.38/
      SAVE PULL
      REAL JJJ(7:12)
      DATA JJJ /1,2,0,3,0,4/
      SAVE JJJ

      INTEGER J, K
      REAL XOFF

      K=IFIX(Z/50.)
      IF(K.LT.7.OR.K.GT.12) RETURN
      J=JJJ(K)
      IF(J.EQ.0) RETURN
      XOFF=XOFFST(J,ITIP)+SLOPE(J,ITIP)*PF
      DX=DX-XOFF
      SIG=SIG*PULL(J,ITIP)

      RETURN
      END



      SUBROUTINE MAGN_LIKEH(XWIN,XDIF,PLKH)

! likelihood calculation

      IMPLICIT NONE

      REAL XWIN, XDIF           ! narrow, wide window half sizes [in]
      REAL PLKH                 ! likelihood that track is real [out]

#include "riter/FAKE_HITS.fi"
      INTEGER I
      REAL S, SI1, BGR, P1, PSI, PBG, STHI, B, G, DX, SIG0
      REAL POIS_LOG
      real SQPI
      PARAMETER (SQPI=2.50662827463)
 
      S=0.
      DO I=1,NT
         IF(ABS(DX_HIT(I)).GT.XWIN) S=S+1.
      ENDDO
      IF(S.EQ.0) S=.5
      SI1=NPASS
      BGR=S*XWIN/(XDIF-XWIN)
      P1=SI1/(SI1+BGR)
      PSI=0.
      PBG=0.
      STHI=0.
      B=1./2./XWIN
      DO I=1,NT
         DX=DX_HIT(I)
         SIG0=SIG_HIT(I)
         IF(ABS(DX).LE.XWIN.AND.SIG0.GT.0) THEN
            G=EXP(-DX**2/2./SIG0**2)/SQPI/SIG0
            PSI=PSI+LOG(P1*G+(1.-P1)*B)
            PBG=PBG+LOG(B)
            STHI=STHI+1.
         ENDIF
      ENDDO
      PSI=PSI+POIS_LOG(SI1*.9+BGR,STHI)
      PBG=PBG+POIS_LOG(BGR*1.2,STHI)
      PLKH=PSI-PBG
      PLKH=EXP(PLKH)
      PLKH=PLKH/(1.+PLKH)

      RETURN
      END



      SUBROUTINE GEDE_CLIP(J0,IROW,XDIF,IR1,IRL,NPASS)

! returns first and last HITB row and increments NPASS,
! if track passed GEDE within (xmin-xdif:xmax+xdif,ymin:ymax)

      IMPLICIT NONE
      INTEGER J0                ! nth superlayer [in]
      INTEGER IROW              ! GEDE row [in]
      REAL XDIF                 ! GEDE x-borders are increased by this [in]
      INTEGER IR1, IRL          ! first and last HITB row, or 0,-1 [out]
      INTEGER NPASS             ! incremented,if track passes active GEDE [out]

#include "riter/FAKE_SUPL.fi"
#include "arte/Arte.fi"
#include "auto/GEDE.fi"
      REAL Z, X, C_ALFA, S_ALFA, DX
      REAL XMIN, XMAX, YMIN, YMAX, ZMIN, ZMAX
#include "arte/Arte.ff"
#include "auto/GEDE.ff"

      IR1=0
      IRL=-1
      ZMIN=FGEDE_ZMIN(IROW)
      ZMAX=FGEDE_ZMAX(IROW)
      C_ALFA=FGEDE_ROT(1,IROW)
      S_ALFA=FGEDE_ROT(4,IROW)
      Z=(ZMIN+ZMAX)/2.
      CALL INTP_TRACK(Z,J0,C_ALFA,S_ALFA)

      YMIN=FGEDE_YMIN(IROW)
      YMAX=FGEDE_YMAX(IROW)
      IF(YP.LT.YMIN.OR.YP.GT.YMAX) RETURN

      XMIN=FGEDE_XMIN(IROW)
      XMAX=FGEDE_XMAX(IROW)
      IF(XP.LT.XMIN-XDIF.OR.XP.GT.XMAX+XDIF) RETURN

      IR1=IGEDE_HIT1(IROW)
      IRL=IGEDE_HITL(IROW)
      IF(XP.GT.XMIN.AND.XP.LT.XMAX.AND.IR1.LE.IRL) NPASS=NPASS+1

      RETURN
      END


      
      SUBROUTINE INTP_TRACK(Z,J0,C_ALFA,S_ALFA)

! quadratic interpolation of a track inside super layer J0
! calculates (x,y,tx,ty)@z in local (GEDE) system

      IMPLICIT NONE
      REAL Z                    ! z 
      INTEGER J0                ! nth superlayer
      REAL C_ALFA, S_ALFA       ! cos, sin of stereo angle
#include "riter/FAKE_SUPL.fi"
      REAL LG(3), XX, C12, C22
      INTEGER K, I

      IF(Z.EQ.ZP.AND.S_ALFA.EQ.ROTA) RETURN

      DO K=1,3
         LG(K)=1.
         DO I=1,3
            IF(I.NE.K) THEN
               LG(K)=LG(K)*(Z-Z_SUPL(I,J0))/(Z_SUPL(K,J0)-Z_SUPL(I,J0))
            ENDIF
         ENDDO
      ENDDO

      ZP=Z
      XP=X_SUPL(1,J0)*LG(1)+X_SUPL(2,J0)*LG(2)+X_SUPL(3,J0)*LG(3)
      YP=Y_SUPL(1,J0)*LG(1)+Y_SUPL(2,J0)*LG(2)+Y_SUPL(3,J0)*LG(3)
      TXP=TX_SUPL(1,J0)*LG(1)+TX_SUPL(2,J0)*LG(2)+TX_SUPL(3,J0)*LG(3)
      TYP=TY_SUPL(1,J0)*LG(1)+TY_SUPL(2,J0)*LG(2)+TY_SUPL(3,J0)*LG(3)
      C11=COV_SUPL(1,1,1,J0)*LG(1)+COV_SUPL(1,1,2,J0)*LG(2)+
     &     COV_SUPL(1,1,3,J0)*LG(3)

      ROTA=S_ALFA
      IF(ROTA.EQ.0) RETURN

      XX= XP*C_ALFA+YP*S_ALFA
      YP=-XP*S_ALFA+YP*C_ALFA
      XP=XX
      XX=  TXP*C_ALFA+TYP*S_ALFA
      TYP=-TXP*S_ALFA+TYP*C_ALFA
      TXP=XX
      C12=COV_SUPL(1,2,1,J0)*LG(1)+COV_SUPL(1,2,2,J0)*LG(2)+
     &     COV_SUPL(1,2,3,J0)*LG(3)
      C22=COV_SUPL(2,2,1,J0)*LG(1)+COV_SUPL(2,2,2,J0)*LG(2)+
     &     COV_SUPL(2,2,3,J0)*LG(3)
      C11=C11*C_ALFA**2+2.*C12*S_ALFA*C_ALFA+C22*S_ALFA**2

      RETURN
      END



      SUBROUTINE KOVAR_MATRIKA(ISUP,K0,COV,FM)

! calculate covariance matrix at next point, given by ISUP,K

      IMPLICIT NONE
      INTEGER ISUP              ! superlayer index
      INTEGER K0                ! point index within superlayer (1,2,3)
      REAL COV(5,5)             ! covariance matrix 
      REAL FM(5,5)              ! propagation matrix

#include "riter/FAKE_SUPL.fi"
      INTEGER I,J,K,L
      REAL C(5,5), QMS(5,5)
      REAL TX, TY, DZ, RADT


      CALL VZERO(QMS,25)
      IF(K0.GT.1.AND.SEKA_SUPL(K0,ISUP)) THEN
         TX=TX_SUPL(K0,ISUP)
         TY=TY_SUPL(K0,ISUP)
         DZ=Z_SUPL(K0,ISUP)-Z_SUPL(K0-1,ISUP)
         RADT=RADT_SUPL(ISUP)
         CALL SIPALNI_DEL(TX,TY,PF,DZ,RADT,QMS)
      ENDIF      

      DO K=1,5
         DO I=1,5
            C(I,K)=0.
            DO J=1,5
               C(I,K)=C(I,K)+FM(I,J)*COV(J,K)
            ENDDO
         ENDDO
      ENDDO
      DO L=1,5
         DO I=1,5
            COV(I,L)=QMS(I,L)
            DO K=1,5
               COV(I,L)=COV(I,L)+C(I,K)*FM(L,K)
            ENDDO
         ENDDO
      ENDDO

      DO K=1,5
         DO I=1,5
            COV_SUPL(I,K,K0,ISUP)=COV(I,K)
         ENDDO
      ENDDO

      RETURN
      END



      SUBROUTINE SIPALNI_DEL(TX,TY,PF,DZ,RADT,QMS)

! scatterig contribution to covariance matrix
 
      IMPLICIT NONE
      REAL TX, TY, PF           ! track parameters [in]
      REAL DZ, RADT             ! track length, rad. thickness [in]
      REAL QMS(5,5)             ! matrix [out]
 
      REAL THICK, CS, COSI, CMS, CXX, CXY, CYY, DL
      INTEGER I
 
      CS=1+TX**2+TY**2
      COSI=SQRT(CS)
      THICK=RADT*COSI
      CMS=(THICK*CS*(13.6E-3*PF)**2)/3.
      CXX=(1+TX**2)*CMS
      CYY=(1+TY**2)*CMS
      CXY=TX*TY*CMS
      DL=DZ*COSI/4.
 
      QMS(1,1)=CXX*(DL**2)/3.
      QMS(1,2)=CXY*(DL**2)/3.
      QMS(1,3)=CXX*DL/2.
      QMS(1,4)=CXY*DL/2.
 
      QMS(2,1)=QMS(1,2)
      QMS(2,2)=CYY*(DL**2)/3.
      QMS(2,3)=CXY*DL/2.
      QMS(2,4)=CYY*DL/2.
 
      QMS(3,1)=QMS(1,3)
      QMS(3,2)=QMS(2,3)
      QMS(3,3)=CXX
      QMS(3,4)=CXY

      QMS(4,1)=QMS(1,4)
      QMS(4,2)=QMS(2,4)
      QMS(4,3)=QMS(3,4)
      QMS(4,4)=CYY
 
      RETURN
      END



      SUBROUTINE GET_MCSUPLAY

! get coordinates of MC super layers (OTR and ITR) from GESL

      IMPLICIT NONE

#include "riter/FAKE_SUPL.fi"
      REAL Z(NUMSUPL)
#include "arte/Arte.fi"
#include "auto/GESL.fi"
      INTEGER IROW, CMP, I
      INTEGER IER
      SAVE IER
#include "arte/Arte.ff"
#include "auto/GESL.ff"
      
      NSUPL=0
      DO IROW=1,NGESL()
         CMP=IGESL_CMP(IROW)
         IF(CMP.EQ.2.OR.CMP.EQ.3) THEN
            IF(IGESL_MFL(IROW).NE.0) THEN
               NSUPL=NSUPL+1
               IF(NSUPL.GT.NUMSUPL) THEN
                  CALL MEPRIC(IER,100,
     &                 'RITER: get_mcsuplay_: NSUPL > NUMSUPL')
                  NSUPL=NUMSUPL
                  GOTO 1
               ENDIF
               GESL_ROW(NSUPL)=IROW
               Z_SUPL(1,NSUPL)=FGESL_Z1(IROW)
               Z_SUPL(3,NSUPL)=FGESL_Z2(IROW)
               Z_SUPL(2,NSUPL)=(Z_SUPL(1,NSUPL)+Z_SUPL(3,NSUPL))/2.
               X0_SUPL(NSUPL)=FGESL_X2(IROW)
               Y0_SUPL(NSUPL)=FGESL_Y2(IROW)
               IF(CMP.EQ.2) THEN
                  XR_SUPL(NSUPL)=12.
                  YR_SUPL(NSUPL)=15.
                  RADT_SUPL(NSUPL)=(Z_SUPL(2,NSUPL)-Z_SUPL(1,NSUPL))/850
               ELSE
                  XR_SUPL(NSUPL)=0.
                  YR_SUPL(NSUPL)=0.
                  X0_SUPL(NSUPL)=25.
                  RADT_SUPL(NSUPL)=(Z_SUPL(2,NSUPL)-Z_SUPL(1,NSUPL))/160
               ENDIF
            ENDIF
         ENDIF
      ENDDO
 1    CONTINUE
      DO I=1,NSUPL
         Z(I)=Z_SUPL(2,I)
      ENDDO
      CALL SORTZV(Z,IND_SUPL,NSUPL,1,0,0)

      RETURN
      END



      SUBROUTINE SKOZI_MAGNET(X,Y,Z,TX,TY,PF,FRKD,Z1,IER)

! track propagation through magnet region by rkmain_

      IMPLICIT NONE
      REAL X,Y,Z,TX,TY          ! initial [in], final [out]
      REAL PF                   ! inv. momentum [in]
      REAL FRKD(5,5)            ! propagation matrix [out]
      REAL Z1                   ! final z [in]
      INTEGER IER               ! 0=ok [out]

      REAL*8 ZOUT, FSTEP, PIN(6), PREC(4), POUT(6), RKD(5,5)
      REAL DZ, ADZ
      REAL DX, DY, DTX, DTY
      DATA DX /.001/, DY /.001/, DTX /.1E-3/, DTY /.1E-3/
      SAVE DX, DY, DTX, DTY
      INTEGER I, K

      DZ=Z1-Z
      ADZ=ABS(DZ)
      IF(ADZ.LT.(.1)) THEN
         X=X+TX*DZ
         Y=Y+TY*DZ
         Z=Z1
         CALL VZERO(FRKD,25)
         FRKD(1,1)=1.
         FRKD(1,3)=DZ
         FRKD(2,2)=1.
         FRKD(2,4)=DZ
         FRKD(3,3)=1.
         FRKD(4,4)=1.
         FRKD(5,5)=1.
         IER=0
         RETURN
      ENDIF

      FSTEP=SIGN(MIN(10.,ADZ),DZ)
      PIN(1)=Z
      PIN(2)=X
      PIN(3)=Y
      PIN(4)=TX
      PIN(5)=TY
      PIN(6)=PF
      ZOUT=Z1
      PREC(1)=DX/ADZ
      PREC(2)=DY/ADZ
      PREC(3)=DTX/ADZ
      PREC(4)=DTY/ADZ

      CALL RKMAIN(2,ZOUT,FSTEP,PIN,PREC,POUT,RKD,IER)
      IF(IER.NE.0) RETURN

      Z=POUT(1)
      X=POUT(2)
      Y=POUT(3)
      TX=POUT(4)
      TY=POUT(5)
      PF=POUT(6)
      DO K=1,5
         DO I=1,5
            FRKD(I,K)=RKD(I,K)
         ENDDO
      ENDDO

      RETURN
      END


