SUBROUTINE FTFHPL
*-- Author : I.O. Skillicorn
      SUBROUTINE FTFHPL(NPLA,IPA,SD,IPP,SDP,
     1    RPCOS, PHZ, DRPCOS, DPHZ, COVP,                               
     1    THET,  RZII,DTHET,  DRZI, COVR, CH, CHR, CHISQ, NDF)          
*                                                                                                             
*     Calling sequence changed. Error in R-z intercept put out                                                
*     Unused arguments removed. Overall 'Chisq' added.                                                        
*                                                                                                             
C     LEAST SQUARES FIT IN R-Z, PHI-Z  HELIX FRAME                                                            
C     UPDATED FROM FT205PP  3/12/91                                                                           
C     INCLUDES PLANARS IN R-Z FIT                                                                             
C     INCLUDES PLANARS IN PHI-Z FIT - REMOVED                                                                 
C     AUTHOR   I.O.SKILLICORN                                                                                 
C     SET FOR FAST FILTER                                                                                     
C     STR LINE FIT IN R-Z                                                                                     
C     STR LINE FIT IN PHI-Z                                                                                   
C                                                                                                             
C     VERTEX CORRECTED - FIT IN HELIX COORDS TO ALLOW FOR AN OFFSET                                           
C                        VERTEX XVV,YVV                                                                       
C                                                                                                             
C    CORRECTED FOR OFFSET RADIALS                                                                             
*                                                                                                             
C     *************************************************************                                           
C     REJECT R-Z FITS WITH UNREASONABLE INTERCEPT    SEE ZCUT                                                 
C     DISABLED 6/6/90                                                                                         
C     ******************************************************                                                  
C     FOR THREE MODULE FIT ONLY   ABS(ZINT) GT ZCUT                                                           
C     CORRECTED TO AVOID DIVIDE CHECK  11/6/90                                                                
C     VARIABLE ANGLE PLANARS  1/11/90                                                                         
*                                                                                                             
*KEEP,FRDIMS.                                                                                                 
      PARAMETER (MAXHTS=200)                                            
      PARAMETER (NUMWPL=36)                                             
      PARAMETER (MAXTRK=200)                                            
      PARAMETER (MXTTRK=900)                                            
      PARAMETER (MAXTR3=200)                                            
      PARAMETER (MAXHPW=2)                                              
      PARAMETER (MAXDIG=2000)                                           
      PARAMETER (NUMRWR=1727)                                           
      PARAMETER (NUMPWR=1151)                                           
*KEEP,FH1WORK.                                                                                                
       COMMON/FGMIOS/                                                   
*    Planar geometry                                                                                          
     + ZPP(140),C(140),S(140),ITYPE(140),WZERO(140),WSPACE,             
*                                                                                                             
*    Radial geometry                                                                                          
     + ZP(36),PHW(36),WS(36)                                            
*                                                                                                             
       COMMON/H1WORK/                                                   
*    Radial data...                                                                                           
     + WW(MAXHTS,36),DRI(MAXHTS,36),RM(MAXHTS,36),                      
     + NDP(36),  NW(MAXHTS,36), DWS(MAXHTS,36),                         
*                                                                                                             
*    Planar Data                                                                                              
     + NDPW(NUMWPL),DW(MAXHTS,NUMWPL),                                  
     + DRIW(MAXHTS,NUMWPL),NDW(MAXHTS,NUMWPL),                          
     + WWP(MAXHTS,NUMWPL),                                              
     + IPHOLE(MAXHTS,NUMWPL),                                           
*                                                                                                             
*    Pointers into DIGI bank for IOS labelled hits                                                            
     +  IPFRRE(MAXHTS,36),IPFRPE(MAXHTS,36),NFRRE,NFRPE,                
     +  IRPIOS(MAXDIG,2), IPPIOS(MAXDIG,2),                             
*                                                                                                             
*    Track segment data                                                                                       
     + NTRAKS(3),IRPT(12,MAXTRK,3),SDRFT(12,MAXTRK,3),                  
*                                                                                                             
*    Fit data                                                                                                 
     + PCOSL(MAXTRK,3),PSINL(MAXTRK,3),PHZL(MAXTRK,3),                  
     + DPCOSL(MAXTRK,3),DPSINL(MAXTRK,3),                               
     + DPHZL(MAXTRK,3),CHSQ(MAXTRK,3),RZI(MAXTRK,3),                    
     + RPCOSG(MAXTRK),RPSING(MAXTRK),                                   
     + PHZG(MAXTRK),CC(3,MAXTRK),ZIG(MAXTRK),                           
     + IRADG(36,MAXTRK),PHIG(36,MAXTRK),                                
     + IG,SDRADG(36,MAXTRK),                                            
     + R1,Z1,RFIT(MAXTRK,3),                                            
     + CHG(MAXTRK),                                                     
     + PPA(MAXTRK,3),  ZZA(MAXTRK,3),                                   
     + GPA(MAXTRK,3),GZA(MAXTRK,3)                                      
*                                                                                                             
*                                                                                                             
*KEEP,FPTVTX.                                                                                                 
      COMMON/VERTVV/ZV ,XVV,YVV                                         
**the common/VERTEX/ becomes /VERTVV/ (in analogy to /VERTFF/) on the                                         
** 17/6/91, since it is in conflict with the VERTEX module (g.bernardi)                                       
** (note that all these common names should start by F in this deck...)                                       
*KEEP,FRWERR.                                                                                                 
      COMMON /WERR/ERRVL,ERRV,ERRP,ERRRX                                
*KEND.                                                                                                        
      COMMON/FVFLAG/IVERTX                                              
      COMMON/CORRXY/CX1,CX2,CX3,CY1,CY2,CY3                             
                                                                        
      DIMENSION XX(80),YY(80),WP(80)                                    
      DIMENSION IPA(36),SD(36),IPP(36),SDP(36),CB(36)                   
      DIMENSION IPAA(36),IPZZ(36),CHHH(36) ,CRRR(36)                    
*                                                                                                             
*                                                                                                             
C     TO USE Z-VERTEX  IVERTX=1                                                                               
C                                                                                                             
C     NOTE THAT XV,YV MUST ALWAYS BE USED TO GET CORRECT HELIX FRAME -                                        
C     NOMINAL BEAM POSITION WOULD BE USED IN PRACTISE .                                                       
*                                                                                                             
C     Z-VERTEX NEED NOT BE USED . IT'S USE RESULTS IN A BETTER DEFINITIO                                      
C     OF THE R-Z SLOPE - FEWER PATREC  ERRORS - BETTER D(1/P)                                                 
C     USE PLANARS IN FINAL PHI-Z FIT TO IMPROVE R   : IPL=1                                                   
      IPL=1                                                             
      IVERTX=0                                                          
*                                                                                                             
C     WRITE(*,*)' FTFHPL ERRORS ',ERRVL,ERRV,ERRP,ERRRX                                                       
      PI2=6.2831853                                                     
C     Z INTERCEPT CUT *******************                                                                     
      ZCUT=1000.                                                        
C     ***********************************                                                                     
C      TO CALCULATE R  USE:                                                                                   
C      R=RD*Z +RO                                                                                             
*                                                                                                             
C     TO CALCULATE PHI USE:                                                                                   
C     PHI=Z*RPCOS+PHZ                                                                                         
*                                                                                                             
*                                                                                                             
      NDF = 0                                                           
      CHISQ = 0.                                                        
*                                                                                                             
C     FIT R-Z    IN HELIX COORDS - NO VERTEX CONSTRAINT                                                       
*                                                                                                             
      II=0                                                              
C*********************************************************                                                    
C     ADD IN VERTEX                                                                                           
      IF(IVERTX.EQ.1)THEN                                               
      II=1                                                              
      XX(II)=ZV                                                         
      YY(II)=0.                                                         
      WP(II)=1./0.02                                                    
      J=0                                                               
C      PRINT1003,II,J,XX(II),YY(II),WP(II)                                                                    
      ENDIF                                                             
C*********************************************************                                                    
*                                                                                                             
      DO 20 K=1,NPLA                                                    
C     J IS POINT NO.     K=PLANE NO.                                                                          
C     RADIALS                                                                                                 
      J=IPA(K)                                                          
      IF(J.EQ.0)GOTO120                                                 
      IF(DRI(J,K).GT.900.)GOTO120                                       
************************************************************************                                      
*     CORRECT FOR ERRORS IN POSITION OF RADIAL HERE                                                           
************************************************************************                                      
      IF(K.GE.1.AND.K.LE.12) THEN                                       
      CX=CX1                                                            
      CY=CY1                                                            
      ENDIF                                                             
      IF(K.GE.13.AND.K.LE.24) THEN                                      
      CX=CX2                                                            
      CY=CY2                                                            
      ENDIF                                                             
      IF(K.GE.25.AND.K.LE.36) THEN                                      
      CX=CX3                                                            
      CY=CY3                                                            
      ENDIF                                                             
*                                                                                                             
*                                                                                                             
      II=II+1                                                           
C     LAB PHI                                                                                                 
      PHILL=ATAN((DRI(J,K)*SD(K)+DWS(J,K))/RM(J,K))+WW(J,K)             
      IF(PHILL.LT.0.0)PHILL=PHILL+PI2                                   
C     LAB R                                                                                                   
      RR=SQRT(RM(J,K)**2+(DRI(J,K)*SD(K)+DWS(J,K))**2)                  
C     CORRECT FOR  DISPLACEMENT OF RADIAL                                                                     
      X=RR*COS(PHILL)-CX                                                
      Y=RR*SIN(PHILL)-CY                                                
C     HELIX R                                                                                                 
      XH=X-XVV                                                          
      YH=Y-YVV                                                          
      RRH=SQRT(XH**2+YH**2)                                             
      YY(II)=RRH                                                        
      XX(II)=ZP(K)                                                      
      WP(II)=1.                                                         
*---------------------------------------------                                                                
C      PRINT1003,II,J,XX(II),YY(II),WP(II)                                                                    
*---------------------------------------------                                                                
 120  CONTINUE                                                          
      IF(IPL.EQ.0)GOTO20                                                
C     PLANARS                                                                                                 
      J=IPP(K)                                                          
      IF(J.EQ.0)GOTO20                                                  
      IF(DRIW(J,K).EQ.900.)GOTO20                                       
      PHI= RPCOS*ZPP(K)+ PHZ                                            
*      AL=ATAN2(S(K),C(K))                                                                                    
C     1/11/90    VARIABLE ANGLE PLANARS                                                                       
      AL=WWP(J,K)                                                       
      TH=PHI-AL                                                         
      AA=ABS(SIN(TH))                                                   
C     TO AVOID DIV. BY 0                                                                                      
      IF(AA.LT.0.1)GOTO20                                               
      II=II+1                                                           
      WWW=DRIW(J,K)*SDP(K)+DW(J,K)                                      
      WWW=WWW-YVV*C(K)+XVV*S(K)                                         
      RR=ABS(WWW)/AA                                                    
      YY(II)=RR                                                         
      XX(II)=ZPP(K)                                                     
      WP(II)=AA/0.025                                                   
*---------------------------------------------                                                                
C      PRINT1003,II,J,XX(II),YY(II),WP(II)                                                                    
 1003 FORMAT(' HELIX R',2I5,3F10.4)                                     
*---------------------------------------------                                                                
 20   CONTINUE                                                          
*                                                                                                             
      CALL FTLFTW(XX,YY,WP,II,0,2,RZS,RZII,ET,DRZS,DRZI,COVR)
C     RZS-SLOPE,RZII-INTERCEPT                                                                                
C      WRITE(*,*)' RZS,RZII ',RZS,RZII                                                                        
C      FOR STOREAGE OF HELIX R                                                                                
       THET=RZS                                                         
       DTHET=DRZS                                                       
*                                                                                                             
C     CALCULATE CHI**2 OF FIT TO STR LINE IN R  -Z                                                            
      CHIL=0.                                                           
      DO 131 K=1,II                                                     
      YYM=YY(K)                                                         
      YYP=XX(K)*RZS+RZII                                                
      CHIL=CHIL+(YYP-YYM)**2*WP(K)**2                                   
 131  CONTINUE                                                          
      NDF   = NDF + II                                                  
      IF(II.NE.2) THEN                                                  
         CHR=CHIL/FLOAT(II-2)                                           
*        For an overall 'Chi-squared'...                                                                      
         CHISQ = CHISQ + CHIL                                           
      ELSE                                                              
         CHR=1000.                                                      
         CHISQ = CHISQ + 1000.                                          
      ENDIF                                                             
*                                                                                                             
*                                                                                                             
*                                                                                                             
*                                                                                                             
*                                                                                                             
C     CALCULATE PHI BASED ON FITTED R  - IN LAB                                                               
C      MAKE A FIT TO  HELIX PHI                                                                               
      II=0                                                              
      DO30 K=1,NPLA                                                     
C     J IS POINT NO.     K=PLANE NO.                                                                          
      J=IPA(K)                                                          
      IF(J.EQ.0)GOTO 30                                                 
      IF(DRI(J,K).GT.900.)GOTO 30                                       
      II=II+1                                                           
C     WRITE(*,*)' FTFHPL J,K, DR RM ',II,J,K,DRI(J,K),RM(J,K)                                                 
      XX(II)=ZP(K)                                                      
*                                                                                                             
*                                                                                                             
************************************************************************                                      
*     CORRECT FOR ERRORS IN POSITION OF RADIAL HERE                                                           
************************************************************************                                      
      IF(K.GE.1.AND.K.LE.12) THEN                                       
      CX=CX1                                                            
      CY=CY1                                                            
      ENDIF                                                             
      IF(K.GE.13.AND.K.LE.24) THEN                                      
      CX=CX2                                                            
      CY=CY2                                                            
      ENDIF                                                             
      IF(K.GE.25.AND.K.LE.36) THEN                                      
      CX=CX3                                                            
      CY=CY3                                                            
      ENDIF                                                             
C***********************************************************************                                      
*                                                                                                             
C     USE  HELIX R - THIS INCLUDES PLANARS IF IPL=1                                                           
C     CONVERT HELIX R TO LAB R                                                                                
      RRH=RZS*ZP(K)+RZII                                                
      PHI=RPCOS*ZP(K)+PHZ                                               
      XL=RRH*COS(PHI)+XVV+CX                                            
      YL=RRH*SIN(PHI)+YVV+CY                                            
      RR=SQRT(XL**2+YL**2)                                              
C***********************************************************************                                      
*                                                                                                             
C     PHI WITH FITTED R  - IN LAB - TO GET MORE ACCURACY IN PHI                                               
C     ..................................................................                                      
*                                                                                                             
      IF(ABS((DRI(J,K)*SD(K)+DWS(J,K))/RR).GT.1.0)THEN                  
C     WRITE(*,*)' FHLX ERROR',(DRI(J,K)*SD(K)+WS(K)),RR                                                       
      YY(II)=0.                                                         
      II=II-1                                                           
      GOTO30                                                            
      ENDIF                                                             
C     LAB PHI WITH FITTED R                                                                                   
      PHI=ASIN((DRI(J,K)*SD(K)+DWS(J,K))/RR)+WW(J,K)                    
      IF(PHI.LT.0.0)PHI=PHI+PI2                                         
C     TO HELIX COORDS                                                                                         
************************************************************************                                      
*     CORRECT FOR ERRORS IN POSITION OF RADIAL HERE                                                           
*     EFFECTIVELY XVV, YVV ARE CHANGED                                                                        
************************************************************************                                      
*                                                                                                             
      XT=RR*COS(PHI)-XVV -CX                                            
      YT=RR*SIN(PHI)-YVV -CY                                            
C     NEXT LINE IS PHI HELIX  CALCULATED WITH FITTED R                                                        
      YY(II)=ATAN2(YT,XT)                                               
      IF(YY(II).LT.0.0)YY(II)=YY(II)+PI2                                
C     RECALCULATE  CORRECTIONS -  USE RESULT OF PREVIOUS FIT                                                  
      PHI=ZP(K)*RPCOS+PHZ                                               
      CORR= 00.                                                         
      IF(ABS(PHI-YY(II)).LT.0.020)CORR=0.                               
      IF(ABS(PHI+PI2-YY(II)).LT.0.020)CORR=-PI2                         
      IF(ABS(PHI-PI2-YY(II)).LT.0.020)CORR=+PI2                         
      YY(II)=YY(II)+CORR                                                
*                                                                                                             
C     ------------------------------------------------------------                                            
C     THE  ERROR SEEMS TO BE CALCULATED CORRECTLY ( ON AVERAGE )                                              
C     FOR 3 AND 2 MODULES EVENTS WITH THE FOLLOWING WEIGHT                                                    
C     ( IT MUST OVERESTIMATE THE ERROR DUE TO DR HOWEVER)                                                     
      WWPP=SQRT((ERRP/RR)**2+                                           
     1((DRI(J,K)*SD(K)+DWS(J,K))/RR**2)**2)                             
      WP(II)=1./WWPP                                                    
*------------------------------------------                                                                   
C      PRINT1002,J,II,XX(II),YY(II),WP(II),RR                                                                 
 1002 FORMAT(' PHI   ',2I5,5F10.4)                                      
*------------------------------------------                                                                   
 30   CONTINUE                                                          
*******                                                                                                       
      IF(II .GT. 1)THEN                                                 
*     Try to repair Phi-discontinuites...                                                                     
      DO 32 JJ=2,II                                                     
       DP =     YY(JJ)-YY(JJ-1)                                         
       IF(DP .GT. 0) THEN                                               
        IF(ABS(DP) .GT. ABS(DP-PI2))YY(JJ) = YY(JJ) - PI2               
       ELSE                                                             
        IF(ABS(DP) .GT. ABS(DP+PI2))YY(JJ) = YY(JJ) + PI2               
       ENDIF                                                            
 32   CONTINUE                                                          
      ENDIF                                                             
*******                                                                                                       
C                                                                                                             
      CALL FTLFTW(XX,YY,WP,II,0,2,RPCOS,PHZ,EL,DRPCOS,DPHZ,COVP)
C     RPCOS-SLOPE PHI-Z,PHZ-INTERCEPT  PHI AXIS                                                               
      IF(RPCOS.EQ.0.0.AND.PHZ.EQ.0.0)THEN                               
       CH=1000.                                                         
       CHISQ = CHISQ + 1000.                                            
       NDF = NDF + II                                                   
       GO TO 500                                                        
      ENDIF                                                             
*                                                                                                             
*------------------------------------------                                                                   
CDEB  PRINT5000,RPCOS,PHZ,DRPCOS,DPHZ                                                                         
 5000 FORMAT(' PHI-Z RPCOS,PHZ,EPC,EP ',4F10.5)                         
*------------------------------------------                                                                   
*                                                                                                             
C     GUARD AGAINST RPCOS<< ERROR                                                                             
****  IF(ABS(RPCOS).LT.DRPCOS/10.)RPCOS=DRPCOS/10.                                                            
C                                                                                                             
C                                                                                                             
C     CALCULATE CHI**2 OF FIT TO STR LINE IN PHI-Z                                                            
      CHIL=0.                                                           
      DO 31 K=1,II                                                      
      YYM=YY(K)                                                         
      IF(YYM.EQ.0.)GOTO31                                               
      YYP=XX(K)*RPCOS+PHZ                                               
      CHIL=CHIL+(YYP-YYM)**2*WP(K)**2                                   
 31   CONTINUE                                                          
                                                                        
      NDF   = NDF + II                                                  
      IF(II.GT.2)THEN                                                   
         CH=CHIL/FLOAT(II-2)                                            
*        For an overall 'Chi-squared'...                                                                      
         CHISQ = CHISQ + CHIL                                           
      ELSE                                                              
         CH = 1000.                                                     
         CHISQ = CHISQ + 1000.                                          
      ENDIF                                                             
                                                                        
 500  CONTINUE                                                          
      IF (NDF .NE. 4) THEN                                              
         CHISQ = CHISQ / FLOAT(NDF-4)                                   
      ENDIF                                                             
                                                                        
C     WRITE(*,*)' RPCOS,PHZ,CH ',RPCOS,PHZ,CH                                                                 
*------------------------------------------                                                                   
C     PRINT 1000,PP,DL,DM,DN,DRPCOS*100.,PCOS,PSIN,PHZ                                                        
 1000 FORMAT('FHLX,P,LMN,D(1/PCOST),PC,PS,PHZ',F8.2,3F8.4,2F10.2,2F8.3) 
*------------------------------------------                                                                   
      RETURN                                                            
      END                                                               
*                                                                                                             
*