*
* $Id$
*
* $Log$
* Revision 1.1  1996/03/06 15:36:26  mclareni
* Initial revision
*
*
#include "geant321/pilot.h"
*CMZ :  3.21/02 29/03/94  15.41.50  by  S.Giani
*-- Author :
      SUBROUTINE GFITKF(IVER,IFAFT)
*
*    trak fitting using GEANE and KALMAN FILTER
*
*    IVER = 0  forward  tracking
*           1  backward tracking
*
*    IFAFT = 0  Fit successful
*    IFAFT = 1  Error matrix singular
*    IFAFT = 9  Fit not successful
*
*    Ask V.Innocente for documentation
*
#include "geant321/gcflag.inc"
#include "geant321/ertrio.inc"
*
*
      PARAMETER (NPLAN = 8, NSTOP = NPLAN-1, NSTOP5 = 5*NPLAN )
      COMMON /GCFIT/
     +          CSIX(6,0:NSTOP),CSIY(6,0:NSTOP),CSIZ(6,0:NSTOP)
     +         ,WW(5,5,0:NSTOP),DDT(5,5,0:NSTOP)
     +         ,VV(5,5,0:NSTOP),SS(5,5,0:NSTOP)
     +         ,CHI2,CHI2N,CHI2T,CHI2M
     +         ,PLANI(3,4,0:NSTOP)
      DOUBLE PRECISION
     +          CSIX           ,CSIY           ,CSIZ
     +         ,WW             ,DDT
     +         ,VV             ,SS
     +         ,CHI2,CHI2N,CHI2T,CHI2M
*
*     CSIZ            measured values
*     CSIY            predicted values
*     CSIX            fitted  values
*
*     VV              Weight on CSIZ
*     WW              tracking Weight
*     SS              fit covariance
*
*     DDT             trasport matrix
*
*     CHI2            first chi2
*     CHI2N           estimated new chi2
*     CHI2T           estimated new chi2 due to tracking
*     CHI2M           estimated new chi2 due to measurements
*
*
      REAL PD(3),RD(15),PC(3),RC(15),H(3),XXI(3),XXO(3),PII(3),POO(3)
      REAL DIF(3)
*
      DOUBLE PRECISION VDUM(5),YDUM(5),AA(5,5),XDU55(5,5)
      DOUBLE PRECISION WI(5,5,0:NSTOP)
      DOUBLE PRECISION CC(5,5,0:NSTOP),DDUM(5)
      DOUBLE PRECISION DZERO, DSMALL, DBIG
*
      CHARACTER * 4 CHOPT
*
      PARAMETER (NITER = 2)
*
      DATA H/0.,0.,20./   ! magnetc field (not really needed)
*
      DATA E1S/1./    ! % error in 1/p
      DATA E2S/.3/    ! error in py/px
      DATA E3S/.3/    ! error in pz/px
*
      DATA DZERO,DSMALL,DBIG/0.D0,.1D-8,1.D8/
*_____________________________________________________________________
*
      IFAFT = 0
*
        IF (IDEBUG.GE.1) THEN
          PRINT *,' GFITKL, NPLAN, NITER ',NPLAN,NITER
        ENDIF
*
      CALL VZERO(RD,15)
*
      CHOPT = 'EP'
      IF (IVER.NE.0) CHOPT = 'BEP'
*
**     compute initial coovariance matrix (to be moved in GEAFIT?)
*
      CALL VZERO(CC,50)
      CALL VZERO(WI,50)
      CC(1,1,0) = (E1S*CSIX(1,0))**2
      CC(2,2,0) = E2S**2
      CC(3,3,0) = E3S**2
      CC(4,4,0) = 1.D0/VV(4,4,0)
      CC(5,5,0) = 1.D0/VV(5,5,0)
*
**     NITER  iterations
*
      DO 101 ITER=1,NITER
*
ccc        IF (ITER.NE.1) CALL UCOPY(SS(1,1,0),CC(1,1,0),50)
        CALL UCOPY(CSIX(1,0),CSIY(1,0),12)
*                    chi2
        DO J = 1,5
          DO K=1,5
            CHI2 =
     +       (CSIX(K,0)-CSIZ(K,0))*VV(K,J,0)*
     +       (CSIX(J,0)-CSIZ(J,0))
          ENDDO
        ENDDO
*            loop over inner planes
        DO JSTOP = 1,NSTOP
*            load starting point
           PD(1) = ABS(CSIX(1,JSTOP-1))
           PD(2) = CSIX(2,JSTOP-1)
           PD(3) = CSIX(3,JSTOP-1)
           XXI(1) = PLANI(1,1,JSTOP-1)*CSIX(4,JSTOP-1)
     +            + PLANI(1,2,JSTOP-1)*CSIX(5,JSTOP-1)
     +            + PLANI(1,4,JSTOP-1)*CSIX(6,JSTOP-1)
           XXI(2) = PLANI(2,1,JSTOP-1)*CSIX(4,JSTOP-1)
     +            + PLANI(2,2,JSTOP-1)*CSIX(5,JSTOP-1)
     +            + PLANI(2,4,JSTOP-1)*CSIX(6,JSTOP-1)
           XXI(3) = PLANI(3,1,JSTOP-1)*CSIX(4,JSTOP-1)
     +            + PLANI(3,2,JSTOP-1)*CSIX(5,JSTOP-1)
     +            + PLANI(3,4,JSTOP-1)*CSIX(6,JSTOP-1)
*
           CH = SIGN(1.D0,CSIX(1,JSTOP-1))
           ITYP = 5
           IF ( CH.LT.0)  ITYP = 6
           SPU = 1.
           CALL TRSDSC(PD,RD,PC,RC,H,CH,IERR,SPU,
     +                 PLANI(1,1,JSTOP-1),PLANI(1,2,JSTOP-1))
*
**         Load coov matrix at plane jstop-1
*
           k=0
           do i=1,5
           do j=i,5
             k = k+1
             FF = 1.D0
             IF (K.NE.1.AND.K.LE.5) FF = CH
             RC(K) = FF*CC(I,J,JSTOP-1)
           enddo
           enddo
*
           CALL DIRCOS(PC(2),DIF)
           P0 = 1.D0/PD(1)
           PII(1) = P0*DIF(1)
           PII(2) = P0*DIF(2)
           PII(3) = P0*DIF(3)
           CALL EUFILP(1,RC,PLANI(1,1,JSTOP-1),PLANI(1,1,JSTOP))
*                   track
           CALL ERTRAK(XXI,PII,XXO,POO,ITYP,CHOPT)
           IF (IEPRED(1).NE.1) THEN
             print *, 'plane ',JSTOP,' not reached '
             IFAFT = 9
             GO TO 999
           ENDIF
*
*
**            load arriving point
*
           CSIY(1,JSTOP) = CH*ERPOUT(1,1)
           CSIY(2,JSTOP) = ERPOUT(2,1)
           CSIY(3,JSTOP) = ERPOUT(3,1)
           CSIY(4,JSTOP) =
     +         PLANI(1,1,JSTOP)*XXO(1)+PLANI(2,1,JSTOP)*XXO(2)+
     +         PLANI(3,1,JSTOP)*XXO(3)
           CSIY(5,JSTOP) =
     +         PLANI(1,2,JSTOP)*XXO(1)+PLANI(2,2,JSTOP)*XXO(2)+
     +         PLANI(3,2,JSTOP)*XXO(3)
           CSIY(6,JSTOP) =
     +         PLANI(1,4,JSTOP)*XXO(1)+PLANI(2,4,JSTOP)*XXO(2)+
     +         PLANI(3,4,JSTOP)*XXO(3)
           K=0
           DO I=1,5
           DO J=I,5
             K=K+1
             FF = 1.D0
             IF (K.NE.1.AND.K.LE.5) FF = CH
             WW(I,J,JSTOP) = FF*ERROUT(K,1)
             WW(J,I,JSTOP) = WW(I,J,JSTOP)
           ENDDO
           ENDDO
C            DO I=1,5
C              IF( WW(I,I,JSTOP).LE.DZERO )  THEN
C                 WW(I,I,JSTOP) = DSMALL
C              ENDIF
C            ENDDO
            CALL UCOPY(WW(1,1,JSTOP),WI(1,1,JSTOP),50)
            CALL DSINV(5,WW(1,1,JSTOP),5,IFAIL)
            IF ( IFAIL.NE.0 )  THEN
              IF ( IDEBUG.GE.1 )
     1           PRINT *,'DSINV IFAIL',IFAIL,' AT PLANE ',JSTOP
                CALL VZERO(WW(1,1,JSTOP),50)
            ENDIF
*
           DO I=1,5
           DO J=1,5
             FF = 1.D0
             IF (I.EQ.1 .AND. J.NE.1 )  FF = CH
             IF (J.EQ.1 .AND. I.NE.1 )  FF = CH
             DDT(I,J,JSTOP) = FF*ERDTRP(J,I,1)
           ENDDO
           ENDDO
*
**        filter
*
          CALL DVADD(25,WW(1,1,JSTOP),WW(2,1,JSTOP),
     +                  VV(1,1,JSTOP),VV(2,1,JSTOP),
     +                  CC(1,1,JSTOP),CC(2,1,JSTOP))
          CALL DSINV(5,CC(1,1,JSTOP),5,IFAIL)
          IF ( IFAIL.NE.0 )  THEN
            IF ( IDEBUG.GE.1 )
     1         PRINT *,'DSINV IFAIL',IFAIL,' AT PLANE ',JSTOP
              CALL VZERO(CC(1,1,JSTOP),50)
          ENDIF
          CALL DMMPY(5,5,VV(1,1,JSTOP),VV(1,2,JSTOP),VV(2,1,JSTOP),
     +                   CSIZ(1,JSTOP),CSIZ(2,JSTOP),
     +                   YDUM(1),      YDUM(2))
          CALL DMMPA(5,5,WW(1,1,JSTOP),WW(1,2,JSTOP),WW(2,1,JSTOP),
     +                   CSIY(1,JSTOP),CSIY(2,JSTOP),
     +                   YDUM(1),      YDUM(2))
          CALL DMMPY(5,5,CC(1,1,JSTOP),CC(1,2,JSTOP),CC(2,1,JSTOP),
     +                   YDUM(1),      YDUM(2),
     +                   CSIX(1,JSTOP),CSIX(2,JSTOP))
          CSIX(6,JSTOP) = CSIY(6,JSTOP)
*                CHI2
          DO J = 1,5
            DO K=1,5
              CHI2 = CHI2 +
     +         (CSIX(K,JSTOP)-CSIZ(K,JSTOP))*VV(K,J,JSTOP)*
     +         (CSIX(J,JSTOP)-CSIZ(J,JSTOP))                 +
     +         (CSIX(K,JSTOP)-CSIY(K,JSTOP))*WW(K,J,JSTOP)*
     +         (CSIX(J,JSTOP)-CSIY(J,JSTOP))
            ENDDO
          ENDDO
*
        ENDDO               !  End plane loop
*
**               smoothing
*
        CALL UCOPY(CC(1,1,NSTOP),SS(1,1,NSTOP),50)
*                chi2
        DO J = 1,5
          DO K=1,5
            CHI2M =
     +         (CSIX(K,NSTOP)-CSIZ(K,NSTOP))*VV(K,J,NSTOP)*
     +         (CSIX(J,NSTOP)-CSIZ(J,NSTOP))
            CHI2T =
     +         (CSIX(K,NSTOP)-CSIY(K,NSTOP))*WW(K,J,NSTOP)*
     +         (CSIX(J,NSTOP)-CSIY(J,NSTOP))
          ENDDO
        ENDDO
*
        DO JS=NSTOP-1,0,-1
          CALL DMMLT(5,5,5,DDT(1,1,JS+1),DDT(1,2,JS+1),DDT(2,1,JS+1),
     1                     WW(1,1,JS+1),WW(1,2,JS+1),WW(2,1,JS+1),
     2                     XDU55(1,1),XDU55(1,2),XDU55(2,1), DDUM )
          CALL DMMLT(5,5,5,CC(1,1,JS),CC(1,2,JS),CC(2,1,JS),
     1                     XDU55(1,1),XDU55(1,2),XDU55(2,1),
     2                     AA(1,1),AA(1,2),AA(2,1), DDUM  )
          CALL DVSUB(25,SS(1,1,JS+1),SS(2,1,JS+1),
     1                  WI(1,1,JS+1),WI(2,1,JS+1),
     2                  XDU55(1,1),XDU55(2,1) )
          CALL DMMLT(5,5,5,XDU55(1,1),XDU55(1,2),XDU55(2,1),
     1                     AA(1,1),AA(2,1),AA(1,2),
     2                     XDU55(1,1),XDU55(1,2),XDU55(2,1), DDUM  )
          CALL UCOPY(CC(1,1,JS),SS(1,1,JS),50)
          CALL DMMLA(5,5,5, AA(1,1),AA(1,2),AA(2,1),
     1                     XDU55(1,1),XDU55(1,2),XDU55(2,1),
     2                     SS(1,1,JS),SS(1,2,JS),SS(2,1,JS) )
          CALL DVSUB(5,CSIX(1,JS+1),CSIX(2,JS+1),
     1                 CSIY(1,JS+1),CSIY(2,JS+1),
     2                 YDUM(1),YDUM(2))
          CALL DMMPA(5,5,AA(1,1),AA(1,2),AA(2,1),
     1                  YDUM(1),YDUM(2),
     2                  CSIX(1,JS),CSIX(2,JS) )
*
          DO I=1,5
            IF (SS(I,I,JS).LE.DZERO) THEN
              IFAFT = 1
              SS(I,I,JS) = DBIG
            ENDIF
          ENDDO
*                    chi2
          DO J = 1,5
            DO K=1,5
              CHI2M = CHI2M +
     +           (CSIX(K,JS)-CSIZ(K,JS))*VV(K,J,JS)*
     +           (CSIX(J,JS)-CSIZ(J,JS))
              CHI2T = CHI2T +
     +           (CSIX(K,JS)-CSIY(K,JS))*WW(K,J,JS)*
     +           (CSIX(J,JS)-CSIY(J,JS))
            ENDDO
          ENDDO
*
        ENDDO
        CHI2N = CHI2T + CHI2M
        print *,' chi2s ',iter,chi2,chi2n,chi2t,chi2m
        IF ( IDEBUG.GE.1 )  THEN
          WRITE(6,'(I6,''     CSIZ '',/(3X,6E15.5))')ITER,CSIZ
          WRITE(6,'(I6,''     CSIX '',/(3X,6E15.5))')ITER,CSIX
          WRITE(6,'(I6,''     CSIY '',/(3X,6E15.5))')ITER,CSIY
        ENDIF
*
          NDOF = 2*nplan-5
          call hfill(1000*iter+1,prob(sngl(chi2),ndof),0.,1.)
          call hfill(1000*iter+2,prob(sngl(chi2n),ndof),0.,1.)
          call hfill(1000*iter+3,prob(sngl(chi2t),ndof),0.,1.)
          call hfill(1000*iter+4,prob(sngl(chi2m),ndof),0.,1.)
        IF ( IDEBUG.GE.1 )  THEN
          PRINT *, ' SS '
          WRITE(6,'(5E15.5)') ((SS(i,i,ip),i=1,5),ip=0,nstop)
        ENDIF
*
*
101   CONTINUE
*
**      done
*
999   CONTINUE
*
      END
