SUBROUTINE FPFYUV
*-- Author :  R. Henderson
      SUBROUTINE FPFYUV(ID1,ID2,ID3,ISM,PCHI,CHISQ,NDF,SOL,EXYSXY)
**: FPFYUV.......SM. Bug fix. Protect against small Chisq for PROB.                                           
**----------------------------------------------------------------------                                      
**: FPFYUV 30205 SM. Add diagnostic histogram                                                                 
**----------------------------------------------------------------------                                      
*                                                                                                             
C                                                                                                             
C                                                                                                             
C------------------------------------------------------------------                                           
C                                                                                                             
C---  THIS ROUTINE PERFORMS A LEAST SQUARES FIT TO A STRAIGHT                                                 
C---  LINE FOR POINTS AT POSITIONS Z(I) MEASURED IN THE Y U V COORDINATE                                      
C---  FRAMES WHERE U COORDINATES ARE AT ANGLE THETA(2) AND V ARE AT                                           
C---  ANGLE THETA(3) TO Y. THETA IS ASSUMED TO BE MEASURED IN THE                                             
C---  CLOCKWISE DIRECTION.                                                                                    
C                                                                                                             
C---  INPUT  :                                                                                                
C                                                                                                             
C---  Y - POINTS FOR FITTING;  4 FROM Y COORDS, 4 FROM U, 4 FROM V.                                           
C---  RESOL - RESOLUTION ON EACH DIGITIZING (IN PRINCIPLE PER WIRE)                                           
C---  ZPLAN(36) - Z COORDINATE OF EACH PLANAR WIRE SET                                                        
C                                                                                                             
C---  OUTPUT  :                                                                                               
C                                                                                                             
C---  PCHI PROBABILITY FROM CHISQUARE                                                                         
C---  SOL(4) X0 Y0 DX/DZ DY/DZ                                                                                
C---  EXYSXY(4,4) COVARIANCE MATRIX TO SOL                                                                    
C                                                                                                             
C------------------------------------------------------------------                                           
C---                                                                                                          
*KEEP,FPPRAM.                                                                                                 
C                                                                                                             
C---  MAXSEG is maximum number of segments per supermodule                                                    
C---  MAXCON is maximum number of amibiguous segments associatable with                                       
C---         one segment                                                                                      
C---  LIMSTO is maximum number of 2 cluster planes intersections to be                                        
C---         stored per supermodule                                                                           
C---  MSEGLM is maximum number of clusters that can be found before                                           
C---         connectivity considered                                                                          
C---  MAXCLU is maximum number of clusters that can be found after                                            
C---         forming non-connected set    MUST BE 50 IF RUN WITH OLD RCW                                      
C---         (cluster = 3/4 digits found in a straight line in one                                            
C---          4-wire orientation)                                                                             
C                                                                                                             
      PARAMETER (MAXSEG = 200)                                          
      PARAMETER (MAXCON = 100)                                          
      PARAMETER (LIMSTO = 5000)                                         
      PARAMETER (MSEGLM = 150)                                          
      PARAMETER (MAXCLU = 50)                                           
C---                                                                                                          
*KEND.                                                                                                        
C---                                                                                                          
*KEEP,FPLSEG.                                                                                                 
C---                                                                                                          
      COMMON /FPLSEG / PW(12,MAXSEG,3)   , PWC(12,MAXSEG,3)     ,       
     1                 PRCHI(MAXSEG,3)   , NFSEG(3)             ,       
     2                 XYDXY(4,MAXSEG,3) , EXYDXY(4,4,MAXSEG,3) ,       
     3                 ZSEG(2,MAXSEG,3)  ,                              
     4                 ASEGIN(MAXSEG,3)  , ISEGIN(5,MAXSEG,3)   ,       
     5                 MASKSG(MAXSEG,3)  , IDGISG(12,MAXSEG,3)          
C---                                                                                                          
*KEND.                                                                                                        
C---                                                                                                          
*KEEP,FPLGEO.                                                                                                 
C---                                                                                                          
      COMMON /FPLGEO/   ZPLAN(36)   , TP(9)   , YP(26)    , PLANE(3,9), 
     1                 RMAX    , RMIN    , YSTART    , YSPACE    ,      
     2                 X0      , Y0      , PZSTRU (8), STAGER   ,       
     3                 RESOL   , ACUT    , CTP(9)    , STP(9)           
C---                                                                                                          
*KEND.                                                                                                        
C---                                                                                                          
*KEEP,FPCLUS.                                                                                                 
      COMMON /FPCLUS/   TC(3,9,MAXCLU) , NTC(9) , TOC(3,9,MAXCLU) ,     
     2                 TCYUV(4,9,MAXCLU), TCYUVW(4,9,MAXCLU)            
C---                                                                                                          
*KEND.                                                                                                        
C---                                                                                                          
*KEEP,FPH1WRK.                                                                                                
      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)                                           
      LOGICAL DRMASK                                                    
      COMMON /H1WORK/                                                   
C--    *KEEP,FPCSEG.                                                                                          
C---                                                                                                          
     3                  TPNORM(3,9,MAXCLU), PCONST(9,MAXCLU)     ,      
     4                 SMLS(4,2,LIMSTO,3) ,                             
C---                                                                                                          
C--    *KEEP,FPDIGI.                                                                                          
     5                 DRSTO(MSEGLM,4),NDRSTO(4),                       
     6               IDIGST(4,MSEGLM),                                  
     7               SEGTAB(MSEGLM,MSEGLM),DRMASK(MSEGLM,4),            
     8               IDCELL(MSEGLM,4),                                  
     9               NSGTAB(MSEGLM),ASGTAB(MSEGLM),                     
     A               RESSTO(MSEGLM,4) ,                                 
C---                                                                                                          
C--    *KEEP,FPDGI.                                                                                           
     B               IDGIST(MSEGLM,4),IDGISM(4,9,MAXCLU)                
     C               ,RCHI(MAXSEG,3) ,                                  
C---                                                                                                          
C--    *KEEP,FPSTID.                                                                                          
     D               IDRSTO(MSEGLM,4),IDYUV(4,9,MSEGLM),                
     E               IDYUVS(12,MAXSEG,3),FREQ(MSEGLM+1) ,               
C---                                                                                                          
C--    *interface to real data                                                                                
     F             NDPW(NUMWPL),DW(MAXHTS,NUMWPL),DWG(MAXHTS,NUMWPL),   
     G             DRIWP(MAXHTS,NUMWPL),DRIWM(MAXHTS,NUMWPL),           
     G             DRIW(MAXHTS,NUMWPL),IPHOLE(MAXHTS,NUMWPL),           
     H             IPFRPE(MAXHTS,36), IPPIOS(MAXDIG,2)                  
C---.                                                                                                         
*KEND.                                                                                                        
C---                                                                                                          
      DIMENSION W(12),Z(12)                                             
      DIMENSION WSUM(3),WZSUM(3),WZ2SUM(3)                              
      DIMENSION WYSUM(3),WYZSUM(3)                                      
      DIMENSION B(4)                                                    
      DIMENSION WORK(16)                                                
      DIMENSION WTEST(4,4)                                              
      DIMENSION MTRIXA(4,4)                                             
      DIMENSION Y(12),YC(12)                                            
      DIMENSION SOL(4),EXYSXY(4,4)                                      
      DIMENSION XSOL(2),YSOL(2),ZSOL(2)                                 
      DIMENSION COST(3),COS2T(3),TANT(3),TAN2T(3),THETA(3)              
      DIMENSION XT(12),YT(12)                                           
      DIMENSION COUNTS(500),CHISQA(12)                                  
C---                                                                                                          
      REAL MTRIXA                                                       
      DOUBLE PRECISION EXYSXY,SOL,B                                     
      REAL CHISQ                                                        
C---                                                                                                          
      SAVE                                                              
C      DATA THETA/ 0.0 , - 1.0471976 , 1.0471976/                                                             
      DATA NEVENT/0/                                                    
      DATA IFIRST/1/                                                    
C---                                                                                                          
C                                                                                                             
C---   INITIALIZATION PER SUPERMODULE                                                                         
C                                                                                                             
      IF( IFIRST .EQ. 1  .OR.  ISM .NE. ISMLAS )THEN                    
      IFIRST = 0                                                        
      ISMLAS = ISM                                                      
C                                                                                                             
C---  SET FOUND PLANAR SEGMENT COUNTER TO ZERO FOR FIRST SUPERMODULE                                          
C                                                                                                             
        IF( ISM .EQ. 1)THEN                                             
           DO 45 IMOD = 1,3                                             
           NFSEG(IMOD) = 0                                              
   45      CONTINUE                                                     
        ENDIF                                                           
C                                                                                                             
C---  ROTATE THETA SO THETA(1) = 0.0                                                                          
C                                                                                                             
      TZERO = - TP( 1 + (ISM-1)*3 )                                     
      DO 43 I = 1,3                                                     
      THETA(I) = - TP( I + (ISM-1)*3 ) - TZERO                          
   43 CONTINUE                                                          
      CTZERO = COS(TZERO)                                               
      STZERO = SIN(TZERO)                                               
C                                                                                                             
C---  HBOOK MONITOR PLOT                                                                                      
C                                                                                                             
C      CALL HBOOK1(10008,'PROB$',50,0.0,1.0,0.0)                                                              
C                                                                                                             
C---  SETUP GEOMETRICAL CONSTANTS                                                                             
C                                                                                                             
      DO 200 I = 1,3                                                    
      COST(I) = COS( THETA(I) )                                         
      COS2T(I) = COST(I)**2                                             
      TANT(I) = TAN( THETA(I) )                                         
      TAN2T(I) = TANT(I)**2                                             
  200 CONTINUE                                                          
      ENDIF                                                             
C                                                                                                             
C---  SETUP Z ARRAY APPROPRIATE TO CURRENT SM                                                                 
C                                                                                                             
      DO 210 I = 1,4                                                    
      DO 211 J = 1,3                                                    
      Z(I + (J-1)*4 ) = ZPLAN( (((ISM-1)*3 + J) - 1)*4  +  I )          
  211 CONTINUE                                                          
  210 CONTINUE                                                          
C                                                                                                             
C---  CENTRE OF PLANES                                                                                        
C                                                                                                             
      ZSUM = 0.0                                                        
      DO 215 I = 1,12                                                   
        ZSUM = ZSUM + Z(I)                                              
  215 CONTINUE                                                          
      ZMEAN = ZSUM/12.0                                                 
      DO 216 I = 1,12                                                   
       Z(I) = Z(I) - ZMEAN                                              
  216 CONTINUE                                                          
C                                                                                                             
C---  FIND Y U V FOR THE CURRENT SEGMENT                                                                      
      IPLAN1 = MOD(ID1,10)                                              
      IPLAN2 = MOD(ID2,10)                                              
      IPLAN3 = MOD(ID3,10)                                              
      ITRCK1 = ID1 / 10                                                 
      ITRCK2 = ID2 / 10                                                 
      ITRCK3 = ID3 / 10                                                 
C                                                                                                             
C---  FIND OFSETS SUCH THAT Y ARRAY IS ON ORDER YUV                                                           
C                                                                                                             
       IND = MOD(IPLAN1,3)                                              
       IF (IND .EQ. 0) IND = 3                                          
       IOFF1 = 2**IND                                                   
       IF (IOFF1 .EQ. 2) IOFF1 = 0                                      
C---                                                                                                          
       IND = MOD(IPLAN2,3)                                              
       IF (IND .EQ. 0) IND = 3                                          
       IOFF2 = 2**IND                                                   
       IF (IOFF2 .EQ. 2) IOFF2 = 0                                      
C---                                                                                                          
       IND = MOD(IPLAN3,3)                                              
       IF (IND .EQ. 0) IND = 3                                          
       IOFF3 = 2**IND                                                   
       IF (IOFF3 .EQ. 2) IOFF3 = 0                                      
C---                                                                                                          
         DO 10 I = 1,4                                                  
C                                                                                                             
C---  Unpack segment Y values identifiers                                                                     
C                                                                                                             
         Y(I+IOFF1) = TCYUV(I,IPLAN1,ITRCK1)                            
         Y(I+IOFF2) = TCYUV(I,IPLAN2,ITRCK2)                            
         Y(I+IOFF3) = TCYUV(I,IPLAN3,ITRCK3)                            
   10    CONTINUE                                                       
C                                                                                                             
C---  SET WEIGHT MATRIX                                                                                       
C                                                                                                             
      DO 20 I=1,4                                                       
      W(I+IOFF1) = TCYUVW(I,IPLAN1,ITRCK1)                              
      W(I+IOFF2) = TCYUVW(I,IPLAN2,ITRCK2)                              
      W(I+IOFF3) = TCYUVW(I,IPLAN3,ITRCK3)                              
   20 CONTINUE                                                          
C                                                                                                             
C---  COUNT NUMBER OF DIGITIZINGS CONTRIBUTING                                                                
C                                                                                                             
      NDIG = 0                                                          
      DO 21 I = 1,12                                                    
      IF(W(I) .EQ. 0.0)GO TO 21                                         
      NDIG = NDIG+1                                                     
   21 CONTINUE                                                          
C                                                                                                             
C---  ZERO SUMS                                                                                               
C                                                                                                             
      DO 30 IO = 1,3                                                    
        WSUM(IO) = 0.0                                                  
        WZSUM(IO) = 0.0                                                 
        WZ2SUM(IO) = 0.0                                                
        WYSUM(IO)=0.0                                                   
        WYZSUM(IO) = 0.0                                                
   30 CONTINUE                                                          
C                                                                                                             
C---  LOOP OVER Z POSITIONS TO FORM SUMS                                                                      
C                                                                                                             
      DO 40 IZ = 1,12                                                   
        IO = ((IZ-1)/4) + 1                                             
C---                                                                                                          
          WSUM(IO) = WSUM(IO) + W(IZ)                                   
          WZSUM(IO) = WZSUM(IO)  +  W(IZ) * Z(IZ)                       
          WZ2SUM(IO) = WZ2SUM(IO)  +  W(IZ) * Z(IZ)**2                  
          WYSUM(IO) = WYSUM(IO)  +  W(IZ) * Y(IZ)                       
          WYZSUM(IO) = WYZSUM(IO)  +  W(IZ) * Y(IZ) * Z(IZ)             
C---                                                                                                          
   40 CONTINUE                                                          
C                                                                                                             
C---  SCALE TERMS 2,3 BY COS2T                                                                                
C                                                                                                             
      DO 50 I =2,3                                                      
      WSUM(I) = COS2T(I) * WSUM(I)                                      
      WZSUM(I) = COS2T(I) * WZSUM(I)                                    
      WZ2SUM(I) = COS2T(I) * WZ2SUM(I)                                  
   50 CONTINUE                                                          
C                                                                                                             
C---  NOW FORM MTRIXA                                                                                         
C                                                                                                             
      MTRIXA(1,1) = TAN2T(2) * WSUM(2) + TAN2T(3) * WSUM(3)             
      MTRIXA(2,2) = WSUM(1) + WSUM(2) + WSUM(3)                         
      MTRIXA(3,3) = TAN2T(2) * WZ2SUM(2) + TAN2T(3) * WZ2SUM(3)         
      MTRIXA(4,4) = WZ2SUM(1) + WZ2SUM(2) + WZ2SUM(3)                   
C---                                                                                                          
      MTRIXA(1,2) = TANT(2) * WSUM(2) + TANT(3) * WSUM(3)               
      MTRIXA(2,1) = MTRIXA(1,2)                                         
      MTRIXA(1,3) = TAN2T(2) * WZSUM(2) + TAN2T(3) * WZSUM(3)           
      MTRIXA(3,1) = MTRIXA(1,3)                                         
      MTRIXA(1,4) = TANT(2) * WZSUM(2) + TANT(3) * WZSUM(3)             
      MTRIXA(4,1) = MTRIXA(1,4)                                         
C---                                                                                                          
      MTRIXA(2,3) = TANT(2) * WZSUM(2) + TANT(3) * WZSUM(3)             
      MTRIXA(3,2) = MTRIXA(2,3)                                         
      MTRIXA(2,4) = WZSUM(1) + WZSUM(2) + WZSUM(3)                      
      MTRIXA(4,2) = MTRIXA(2,4)                                         
C---                                                                                                          
      MTRIXA(3,4) = TANT(2) * WZ2SUM(2) + TANT(3) * WZ2SUM(3)           
      MTRIXA(4,3) = MTRIXA(3,4)                                         
C                                                                                                             
C---  NOW CALCULATE ERROR MATRIX FOR XY,SLOPE XY                                                              
C                                                                                                             
      DO 60 IR=1,4                                                      
      DO 70IC=1,4                                                       
        EXYSXY(IR,IC) = MTRIXA(IR,IC)                                   
   70 CONTINUE                                                          
   60 CONTINUE                                                          
C---                                                                                                          
      CALL DINV(4,EXYSXY,4,WORK,IFAIL)                                                                 
C---                                                                                                          
      IF(IFAIL .NE. 0) THEN                                             
      WRITE(6,*)'YUV FIT FAILED'                                        
      RETURN                                                            
      ENDIF                                                             
C                                                                                                             
C---  CALCULATE VECTOR B                                                                                      
C                                                                                                             
      B(1) = TANT(2)*COST(2) *  WYSUM(2) + TANT(3)*COST(3) * WYSUM(3)   
      B(2) = WYSUM(1) + COST(2) * WYSUM(2) + COST(3) * WYSUM(3)         
      B(3) = TANT(2)*COST(2) * WYZSUM(2) + TANT(3)*COST(3) * WYZSUM(3)  
      B(4) =  WYZSUM(1) + COST(2) * WYZSUM(2) + COST(3) * WYZSUM(3)     
C                                                                                                             
C---  NOW SOLVE FOR X,Y,SX,SY                                                                                 
C                                                                                                             
      DO 90 IR =1,4                                                     
      SOL(IR) = 0.0                                                     
   90 CONTINUE                                                          
      DO 100 IR = 1,4                                                   
      DO 110 IC = 1,4                                                   
         SOL(IR) = SOL(IR) + EXYSXY(IR,IC)*B(IC)                        
  110 CONTINUE                                                          
  100 CONTINUE                                                          
C                                                                                                             
C---  PUT ZERO BACK TO Z=0                                                                                    
C                                                                                                             
      SOL(1) = SOL(1) - SOL(3)*ZMEAN                                    
      SOL(2) = SOL(2) - SOL(4)*ZMEAN                                    
      DO 632 I =1,12                                                    
        Z(I) = Z(I) + ZMEAN                                             
 632  CONTINUE                                                          
C--                                                                                                           
      CALL FPPPTZ(EXYSXY,-ZMEAN)
C                                                                                                             
C---   NOW CALCULATE RESULTANT Y U V                                                                          
C                                                                                                             
      DO 130 IZ = 1,4                                                   
      YC(IZ) = SOL(2) + SOL(4)*Z(IZ)                                    
      YC(IZ+4) = COST(2) * ( (SOL(2) + SOL(4)*Z(IZ+4) ) +               
     1             TANT(2) * (SOL(1) + SOL(3)*Z(IZ+4) ) )               
      YC(IZ+8) = COST(3) * ( (SOL(2) + SOL(4)*Z(IZ+8) ) +               
     1             TANT(3) * (SOL(1) + SOL(3)*Z(IZ+8) ) )               
  130 CONTINUE                                                          
C                                                                                                             
C---  CALCULATE CHISQUARE                                                                                     
C                                                                                                             
      CHISQ = 0.0                                                       
C                                                                                                             
C---                                                                                                          
C                                                                                                             
      DO 140 IZ=1,12                                                    
      CHISQA(IZ) = (Y(IZ)-YC(IZ))**2*W(IZ)                              
      CHISQ = CHISQ + (Y(IZ)-YC(IZ))**2*W(IZ)                           
  140 CONTINUE                                                          
      NDF = NDIG-4                                                      
*     Fix for v. small Chisq...                                                                               
      IF(CHISQ .LT. 0.001) THEN                                         
       PCHI = 0.9999999                                                 
      ELSE                                                              
       PCHI = PROB( ABS(CHISQ),NDF )                                    
      ENDIF                                                             
                                                                        
C                                                                                                             
C---  PLSEG ARRAY FILLED HERE                                                                                 
C                                                                                                             
          IF (NFSEG(ISM) .GE. MAXSEG) THEN                              
            CALL ERRLOG(213,'W:FPFYUV: Max segments exceeded')                                         
            RETURN                                                      
          ELSE                                                          
C                                                                                                             
C---  INCREMENT FOUND SEGMENT COUNTER                                                                         
C                                                                                                             
*     IF(PCHI .GT. 0.0001)THEN                                                                                
      IF(PCHI .GT. 0.0000)THEN                                          
        NFSEG(ISM) = NFSEG(ISM) + 1                                     
      ELSE                                                              
        RETURN                                                          
      ENDIF                                                             
C                                                                                                             
C---  Store away digitization pointers for found segment                                                      
C                                                                                                             
      DO 305 I=1,4                                                      
      IDGISG(I+IOFF1,NFSEG(ISM),ISM) = IDGISM(I,IPLAN1,ITRCK1)          
      IDGISG(I+IOFF2,NFSEG(ISM),ISM) = IDGISM(I,IPLAN2,ITRCK2)          
      IDGISG(I+IOFF3,NFSEG(ISM),ISM) = IDGISM(I,IPLAN3,ITRCK3)          
  305 CONTINUE                                                          
C                                                                                                             
C---                                                                                                          
      DO 300 I = 1,12                                                   
C                                                                                                             
C---  PREPARE OUTPUT BANKS                                                                                    
C                                                                                                             
      PW(I,NFSEG(ISM),ISM) = Y(I)                                       
      PWC(I,NFSEG(ISM),ISM) = YC(I)                                     
  300 CONTINUE                                                          
      PRCHI(NFSEG(ISM),ISM) = PCHI                                      
C                                                                                                             
C---  ROTATE BACK FROM THETA(1) = 0.0 TO X Y FRAME                                                            
C                                                                                                             
        IF(TZERO .NE. 0.0)THEN                                          
        S1 =   CTZERO*SOL(1) + STZERO*SOL(2)                            
        S2 = - STZERO*SOL(1) + CTZERO*SOL(2)                            
        S3 =   CTZERO*SOL(3) + STZERO*SOL(4)                            
        S4 = - STZERO*SOL(3) + CTZERO*SOL(4)                            
        SOL(1) = S1                                                     
        SOL(2) = S2                                                     
        SOL(3) = S3                                                     
        SOL(4) = S4                                                     
        ENDIF                                                           
      DO 301 I = 1,4                                                    
                                                                        
        XYDXY(I,NFSEG(ISM),ISM) = SOL(I)                                
          DO 302 J = 1,4                                                
            EXYDXY(I,J,NFSEG(ISM),ISM) = EXYSXY(I,J)                    
  302     CONTINUE                                                      
  301 CONTINUE                                                          
      ZSEG(1,NFSEG(ISM),ISM) = Z(1)                                     
      ZSEG(2,NFSEG(ISM),ISM) = Z(12)                                    
C                                                                                                             
C---  Store segments information for FPSGRF                                                                   
C                                                                                                             
      ISEGIN(1,NFSEG(ISM),ISM)=ID1                                      
      ISEGIN(2,NFSEG(ISM),ISM)=ID2                                      
      ISEGIN(3,NFSEG(ISM),ISM)=ID3                                      
      ISEGIN(4,NFSEG(ISM),ISM)=NDF                                      
      ASEGIN(  NFSEG(ISM),ISM)=CHISQ                                    
C---                                                                                                          
          ENDIF                                                         
      END                                                               
*