C
C
C NOTE: THIS ROUTINE DOES NOT SEEMS TO WORK CORRECTLY, HAS TO BE CHECKED CAREFULLY
C
C
C---------------------------------------------------------------------
      SUBROUTINE SELFTRIG
C---------------------------------------------------------------------
C
      IMPLICIT NONE
C
      INCLUDE 'INTEST.TXT'
C
      REAL PI,calwidth,ztopx,ztopy,zbotx,zboty,MIP2GEV
      real wcorr
      parameter (wcorr=1.0)
      PARAMETER (MIP2GEV=0.0001059994) 
      PARAMETER (PI=3.14159265358979324)
CC      PARAMETER (calwidth=24.2)
      PARAMETER (calwidth=24.1)
      PARAMETER (ztopx=-26.18)
      PARAMETER (ztopy=-26.76)
      PARAMETER (zbotx=-45.17)
      PARAMETER (zboty=-45.75)
C      
      INTEGER i,j,it
      INTEGER ifin,finpar,finpar1,plent,plentx,plenty
      INTEGER xncnt(22),yncnt(22),Nfitx,Nfity
      INTEGER xncnt2(5,22),yncnt2(5,22)
C
      REAL xecnt2(5,22),xwght2(5,22),xcorr2(5,22)
      REAL yecnt2(5,22),ywght2(5,22),ycorr2(5,22)
      REAL ax2(4),bx2(4),eax2(4),ebx2(4)
      REAL ay2(4),by2(4),eay2(4),eby2(4)
      REAL xEmax3(22),yEmax3(22),xmax(22),ymax(22),zx(22),zy(22)
      REAL xecnt(22),xwght(22),xcorr(22)
      REAL yecnt(22),ywght(22),ycorr(22)
      REAL ax,bx,eax,ebx,chi2x,qxp,posixmd
      REAL ay,by,eay,eby,chi2y,qyp,posiymd
      REAL thxm,thym,tmisd,dthxm,dthym,dtmisd,pmisd,pmid
      REAL enet,parze,parz(2,22),ffla,zetamx,zetamy,fflaco,parzen2
      REAL parzen3,funcor2
      REAL CORRANG,ENCORR
C
      COMMON / slftrig / tmisd,ax,bx,eax,ebx,chi2x,Nfitx,ay,by,eay,eby,
     &     chi2y,Nfity,parzen3
      SAVE / slftrig /
C
      COMMON / debug / xncnt2,yncnt2,xecnt2,xwght2,xcorr2,yecnt2,ywght2,
     &     ycorr2,ax2,bx2,eax2,ebx2,ay2,by2,eay2,eby2,zx,zy
      SAVE / debug /     
C
c     Energy calculation
C
      enet = 0.
      parze = 0.
      finpar = 1
      finpar1 = 1
      CALL VZERO(parz,2*22)
      DO i = 1,22
         DO j = 1,96
            parz(1,i) = parz(1,i) + DEXY(1,i,j) ! sum up the energy in each x-plane
            parz(2,i) = parz(2,i) + DEXY(2,i,j) ! sum up the energy in each y-plane
            enet = enet + DEXY(1,i,j) + DEXY(2,i,j) ! sum up the total energy
         ENDDO
         IF (parz(1,i).GE.parze) THEN ! find plane with max energy
            parze = parz(1,i)   ! the energy
            finpar = i          ! the plane number
            finpar1 = 1         ! set x or y indicator 
         ENDIF   
         IF (parz(2,i).GE.parze) THEN
            parze = parz(2,i)
            finpar = i
            finpar1 = 2
         ENDIF              
      ENDDO 
      ffla = FLOAT(finpar)
      IF (finpar1.EQ.1) THEN
         ffla = ffla - 1.
      ENDIF
C      
c     Find the center of the 3 strips with maximum total energy in a plane
C
      CALL VZERO(xemax3,22)
      CALL VZERO(xncnt,22)
      CALL VZERO(yemax3,22)
      CALL VZERO(yncnt,22)
      DO i = 1,22
         DO j = 1,94

            IF ((DEXY(1,i,j)+
     &           DEXY(1,i,j+1)+
     &           DEXY(1,i,j+2)).GT.xemax3(i)) THEN
               xemax3(i)=DEXY(1,i,j)+
     &                   DEXY(1,i,j+1)+
     &                   DEXY(1,i,j+2)
               xncnt(i)=j+1
            ENDIF
            IF ((DEXY(2,i,j)+
     &           DEXY(2,i,j+1)+
     &           DEXY(2,i,j+2)).GT.yemax3(i)) THEN
               yemax3(i)=DEXY(2,i,j)+
     &                   DEXY(2,i,j+1)+
     &                   DEXY(2,i,j+2)
               yncnt(i)=j+1
            ENDIF

         ENDDO
      ENDDO
C
c     Calculate the position of the center strip and the center of 
c     energy (CoE) of 4 surrounding strips
C
      DO i=1,22
         CALL STRIP2POS(1,i,xncnt(i),xmax(i),zx(i))
         CALL STRIP2POS(2,i,yncnt(i),ymax(i),zy(i))
         CALL ENCENT2(1,xncnt(i),i,4,xecnt(i),xwght(i))
         CALL ENCENT2(2,yncnt(i),i,4,yecnt(i),ywght(i))
         xecnt2(1,i)=xecnt(i)
         xwght2(1,i)=xwght(i)
         xncnt2(1,i)=xncnt(i)
         yecnt2(1,i)=yecnt(i)
         ywght2(1,i)=ywght(i)
         yncnt2(1,i)=yncnt(i)
      ENDDO
C      
      dtmisd=1.0
      dthxm=1.0
      dthym=1.0
      tmisd=0
      thxm=0
      thym=0
C
c     Iterative procedure starts here
C
      DO it=1,4
C
c     Fit the CoEs with a linear function
C
         CALL LEASTSQR(22,zx,xecnt,xwght,ax,bx,eax,ebx,chi2x,Nfitx) !<----- LINEAR FIT
         CALL LEASTSQR(22,zy,yecnt,ywght,ay,by,eay,eby,chi2y,Nfity)
         ax2(it)=ax
         ay2(it)=ay
         eax2(it)=eax
         eay2(it)=eay
         bx2(it)=bx
         by2(it)=by
         ebx2(it)=ebx
         eby2(it)=eby
C
c     Calculate theta and phi
C
         dtmisd=ABS(tmisd-ATAN(SQRT((bx*bx)+(by*by))))
         dthxm=ABS(thxm-ABS(ATAN(bx)))
         dthym=ABS(thym-ABS(ATAN(by)))
         tmisd=ATAN(SQRT((bx*bx)+(by*by)))
         thxm=ABS(ATAN(bx))
         thym=ABS(ATAN(by))
         IF (bx.EQ.0..AND.by.GT.0.) pmisd = 90.*(PI/180.)
         IF (bx.EQ.0..AND.by.LT.0.) pmisd = 270.*(PI/180.)
         IF (by.EQ.0..AND.bx.GE.0.) pmisd = 0.*(PI/180.)
         IF (by.EQ.0..AND.bx.LT.0.) pmisd = 180.*(PI/180.)
         IF (by.NE.0..AND.bx.NE.0.) THEN
            pmid = ATAN(by/bx)
            IF (by.LT.0..AND.bx.GT.0.) pmisd = pmid + 360.*(PI/180.)
            IF (bx.LT.0.) pmisd = pmid + 180.*(PI/180.)
            IF (by.GT.0..AND.bx.GT.0.) pmisd = pmid
         ENDIF
C
c     Calculate the position of the strip closest to the fitted line and 
c     the CoE of 4 surrounding strips.
C
         DO i=1,22
            CALL POS2STRIP(1,i,ax+bx*zx(i),xncnt(i))
            CALL POS2STRIP(2,i,ay+by*zy(i),yncnt(i))
            IF (xncnt(i).GE.1.AND.xncnt(i).LE.96) THEN
               CALL ENCENT2(1,xncnt(i),i,4,xecnt(i),xwght(i))
            ELSE
               xecnt(i)=-99.
               xwght(i)=1.0e5
            ENDIF
C            
            IF (yncnt(i).GE.1.AND.yncnt(i).LE.96) THEN
               CALL ENCENT2(2,yncnt(i),i,4,yecnt(i),ywght(i))
            ELSE
               yecnt(i)=-99.
               ywght(i)=1.0e5
            ENDIF
            xncnt2(it+1,i)=xncnt(i)
            yncnt2(it+1,i)=yncnt(i)
            xecnt2(it+1,i)=xecnt(i)
            xwght2(it+1,i)=xwght(i)
            yecnt2(it+1,i)=yecnt(i)
            ywght2(it+1,i)=ywght(i)
         ENDDO        
C
c     Calculate at which plane the particle has entered the calorimeter
c     according to the fit
C
         IF (bx.GT.0.) CALL POS2PLANE((calwidth/2)/bx-ax/bx,plentx)
         IF (bx.LT.0.) CALL POS2PLANE(-(calwidth/2)/bx-ax/bx,plentx)
         IF (bx.EQ.0.) plentx=1
         IF (by.GT.0.) CALL POS2PLANE((calwidth/2)/by-ay/by,plenty)
         IF (by.LT.0.) CALL POS2PLANE(-(calwidth/2)/by-ay/by,plenty)
         IF (by.EQ.0.) plenty=1
         plent=MAX(plentx,plenty)         
C
c     Calculate the projection in the bottom plane
C
         qxp=ax+bx*ztopx
         qyp=ay+by*ztopy
         zetamx=ztopx-zbotx
         zetamy=ztopy-zboty
C         
        IF (qxp.GT.calwidth/2) THEN
           zetamx = zetamx-(qxp-calwidth/2)/bx
           qxp = calwidth/2
        ENDIF
        IF (qxp.LT.-calwidth/2) THEN
           zetamx = zetamx-(qxp+calwidth/2)/bx
           qxp = -calwidth/2
        ENDIF
        IF (qyp.GT.calwidth/2) THEN
           zetamy = zetamy-(qyp-calwidth/2)/by
           qyp = calwidth/2
        ENDIF
        IF (qyp.LT.-calwidth/2) THEN
           zetamy = zetamy-(qyp+calwidth/2)/by
           qyp = -calwidth/2
        ENDIF
C
        posixmd = qxp - zetamx*TAN(tmisd)*COS(pmisd)
        posiymd = qyp - zetamy*TAN(tmisd)*SIN(pmisd)
C
c     Energy correction 
C
         IF ((ABS(ax+bx*zbotx).LE.10.1).AND.
     &        (ABS(ay+by*zboty).LE.10.1)) THEN
            ifin = finpar + 3 
            IF (ifin.GT.22) ifin = 22
         ELSE
            ifin = finpar
         ENDIF
         
         parzen2 = 0.0
         DO i=1,ifin
            parzen2=parzen2+parz(1,i)+parz(2,i) !Sum up energy until 'ifin'
         ENDDO
         
         fflaco = ifin-plent 
         funcor2=parzen2/ENCORR(fflaco/COS(tmisd))
         
         IF ((ABS(ax+bx*zbotx).LE.10.1).AND.
     &        (ABS(ay+by*zboty).LE.10.1)) THEN
            parzen3 = funcor2*(1.-.01775-funcor2*(.2096E-4)+
     &           funcor2*funcor2*funcor2*(.2865E-9))
         ELSE
            parzen3 = funcor2*(1.-.124+funcor2*(.24E-3)+
     &           funcor2*funcor2*funcor2*(.25E-9))/.7
         ENDIF
C
c     Angle correction
C
         DO i=1,22         
C           
c           x view            
C
            IF (xecnt(i).GT.-97.) THEN
               IF (parzen3.GE.250.) THEN
                  xcorr(i) = wcorr*CORRANG(FLOAT(i)/ffla,(thxm*180./PI)+
     &                 (parzen3-250)*1.764705E-2)
               ELSE
                  xcorr(i) = wcorr*CORRANG(FLOAT(i)/ffla,thxm*180./PI)
               ENDIF
            ELSE
               xcorr(i) = 0.0
            ENDIF
            xecnt(i) = xecnt(i) + xcorr(i)
C
c           y view   
C
            IF (yecnt(i).GT.-97.) THEN
               IF (parzen3.GE.250.) THEN
                  ycorr(i) = wcorr*CORRANG(FLOAT(i)/ffla,thym*180./PI+
     &                 (parzen3-250)*1.764705E-2)
               ELSE
                  ycorr(i) = wcorr*CORRANG(FLOAT(i)/ffla,thym*180./PI)
               ENDIF         
            ELSE
               ycorr(i) = 0.0
            ENDIF
            yecnt(i) = yecnt(i) + ycorr(i)
C            
            xcorr2(it,i)=xcorr(i)
            ycorr2(it,i)=ycorr(i)
C                        
         ENDDO
C         
      ENDDO
C     
      RETURN
C
      END

C      
C-----------------------------------------------------------------------
      SUBROUTINE ENCENT(view,nsmax,nplane,nit,ecnt,weight)
c
c Calculates the center of energy in a cluster 
c
C-----------------------------------------------------------------------
C
      IMPLICIT NONE
C
      INCLUDE 'INTEST.TXT'
C
      REAL ecnt,weight,esumw,esum,strip1,strip2,xypos,zpos,xymean,
     &     xystd
      INTEGER nsmax,st0,st1,st2,st3,nplane,nit,view,npos,p
C
      PARAMETER (strip1=17.,strip2=9.)
C
      st0 = NINT(strip1)                ! =17 cluster width for iteration 1
      st1 = NINT(strip1-strip2)         ! =8 
      st2 = NINT((strip1-1.)/2. + 1.)   ! =9 cluster width for iteration 2
      st3 = NINT(FLOAT(st1)/2.)         ! =4
C
      IF (nsmax.GT.0.) THEN
         esumw = 0.
         esum = 0.
         xymean = 0.
         xystd = 0.
         DO P = 1, (st0-(nit-1)*st1) ! loop to 17(9) for iteration 1(2)
C
c     Calculate strip number
C
            IF (nsmax.LT.(st2-st3*(nit-1))) THEN ! if nsmax < 9(5) for iteration 1(2)
               npos = P
               IF (npos.GT.(st0-ABS(nsmax-st2-st3*(nit-1))))
     &              GO TO 7100  ! quit loop after the 8th(4th) strip to the right of nsmax for iteration 1(2) 
            ELSE  
               npos = nsmax+P-st2+st3*(nit-1) ! npos=nsmax-8,-7, ... +7,+8(nsmax-4,-3, ... +3,+4) for iteration 1(2)
               IF (npos.GT.96)
     &              GO TO 7100 ! quit loop after the 96th strip
            ENDIF   
C            
c     Get the position for npos
C
            CALL STRIP2POS(view,nplane,npos,xypos,zpos)            
C
c     Sum up energies
C
            esumw = esumw + xypos*DEXY(view,nplane,npos)
            esum = esum + DEXY(view,nplane,npos)
         ENDDO
C
 7100    CONTINUE
C
c     Calculate CoE and wieghts
C
         IF (esum.GT.0.) THEN
            ecnt = esumw/esum
            weight = ecnt/(esum**.79)
         ELSE
            ecnt = -98.
            weight = 1E5                   
         ENDIF     
      ELSE
         ecnt = -97. 
         weight = 1E5
      ENDIF
C
      RETURN  
C
      END
C
C-----------------------------------------------------------------------
      SUBROUTINE ENCENT2(view,nsmax,nplane,width,ecnt,weight)
C-----------------------------------------------------------------------
C      
      IMPLICIT NONE
C
      INCLUDE 'INTEST.TXT'
C      
      INTEGER i,nplane,view,nsmax,width,nrec
      REAL mx,mx2,s1,s2,xypos,ecnt,weight,zpos
C      
      mx=0.0
      mx2=0.0
      s2=0.0
      nrec=0
      DO i=nsmax-width,nsmax+width
C
         IF(i.GT.0.AND.i.LT.97.AND.DEXY(view,nplane,i).GT.0.0) THEN
C            
            nrec=nrec+1
C
            CALL STRIP2POS(view,nplane,i,xypos,zpos)
C            
            s1=s2
            s2=s2+DEXY(view,nplane,i)
            mx=(mx*s1+xypos*DEXY(view,nplane,i))/s2
            mx2=(mx2*s1+(xypos**2)*DEXY(view,nplane,i))/s2
C
         ENDIF
C         
      ENDDO
C      
      IF (nrec.GT.0) THEN
         ecnt=mx
         IF (nrec.EQ.1) weight=0.244/sqrt(12*s2)
         IF (nrec.NE.1) weight=sqrt(mx2-mx**2)/sqrt(s2)
      ELSE
         ecnt=-99.0
         weight=100000.0
      ENDIF
C
      RETURN
C           
      END
      
C---------------------------------------------------------------------------
      SUBROUTINE POS2PLANE(pos,plane)
c
c Find the plane closest to a certain z position
C---------------------------------------------------------------------------

      IMPLICIT NONE

      REAL pos, npos, pdiff, xy
      INTEGER view, plane, nplane

      pdiff=1000.
      plane=1
      DO view=1,2
         DO nplane=1,22
            CALL STRIP2POS(view,nplane,1,xy,npos)
            IF (ABS(pos-npos).LT.pdiff) THEN
               pdiff=ABS(pos-npos)
               plane=nplane
            ENDIF
         ENDDO
      ENDDO
C
      RETURN
C
      END

C---------------------------------------------------------------------------
      SUBROUTINE POS2STRIP(view,nplane,pos,strip)
c
c Find the strip closest to a certain x or y position
C---------------------------------------------------------------------------
C
      IMPLICIT NONE
C
      REAL pos, npos, minpos, maxpos, pdiff, z
      INTEGER view, nplane, strip, i
C
      pdiff=1000.
      strip=-1
      CALL STRIP2POS(view,nplane,1,minpos,z)
      CALL STRIP2POS(view,nplane,96,maxpos,z)
      IF (pos.LT.minpos) THEN
         strip=0
      ELSEIF (pos.GT.maxpos) THEN
         strip=97
      ELSE
         DO i=1,96
            CALL STRIP2POS(view,nplane,i,npos,z)
            IF (ABS(pos-npos).LT.pdiff) THEN
               pdiff=ABS(pos-npos)
               strip=i
            ENDIF
         ENDDO
      ENDIF
C
      RETURN
C      
      END

C---------------------------------------------------------------------
      SUBROUTINE STRIP2POS(view,nplane,nstrip,xy,z)
c
c Calculates the x (or y) and z position of a strip in PAMELA coordinates
c
c Arguments
c ---------
c view:   x=1 and y=2
c nplane: plane number 1-22
c nstrip: strip number 1-96
c
c Return values
c -------------
c xy: x or y position of the strip-center in PAMELA coordinates
c z:  z position of the plane in PMAELA coordinates
C---------------------------------------------------------------------

      IMPLICIT NONE

      INTEGER view,isy,iseven,nplane,nstrip,nwaf,wstr,npl
      REAL wpos,pos,x,y,z,xy
       
c     Calculate the x- or y-position in a plane

      nwaf=int((nstrip-1)/32) !what wafer (0-2)
      wstr=mod(nstrip-1,32) !what strip in the wafer (0-31)
      wpos=0.096+0.122+wstr*0.244 !the position of the strip center in the wafer
c      pos=wpos+nwaf*8.0005-12.0005 !the position in the plane (oigo shifted to center of plane)
      pos=wpos+nwaf*8.0505-12.0005 !the position in the plane (oigo shifted to center of plane)
     
c     Calculate z position and add x-y-offset depending on the plane number

      isy=view-1 !isy=0 for x and 1 for y
      iseven=mod(nplane,2)! iseven=0 for odd and 1 for even planes
      npl=nplane-1
      IF (isy.EQ.0) THEN
         IF (iseven.EQ.0) THEN
c            print *,'CALCULATING X-ODD'
            x = pos+0.05 !x-odd
            y = pos-0.2
            z = -26.762-0.809*npl-0.2*(npl-1)/2
         ELSE
c            print *,'CALCULATING X-EVEN'
            x = pos-0.05 !x-even
            y = pos+0.0
            z = -26.762-0.809*npl-0.2*npl/2
        ENDIF
        xy=x
      ELSE
         IF (iseven.EQ.0) THEN
c            print *,'CALCULATING Y-ODD'
            x = pos-0.1 ! y-odd
            y = pos-0.15
            z = -26.181-0.809*npl-0.2*(npl-1)/2             
         ELSE
c            print *,'CALCULATING Y-EVEN'
            x =  pos+0.1 ! y-even
            y =  pos-0.05
            z = -26.181-0.809*npl-0.2*npl/2
        ENDIF
        xy=y
      ENDIF
C
      RETURN
      END

C---------------------------------------------------------------------------
      SUBROUTINE LEASTSQR(N,x,y,sy,a,b,ea,eb,chi2,Nfit)
c
c Linear least square fit (method is described in "Taylor" chapter 8)
c
c
c
C---------------------------------------------------------------------------
C
      INTEGER N,Nfit
      REAL x(N),y(N),sy(N),w(N),Swx2,Swx,Swy,Swxy,Sw,a,b,ea,eb,chi2
C
      Swx2=0
      Swx=0
      Swy=0
      Swxy=0
      Sw=0
      Nfit=0
      DO i=1,N
         IF (y(i).GT.-97.) THEN
            Nfit=Nfit+1
            w(i)=1/(sy(i)**2) ! the weight
            Swx2=Swx2+w(i)*(x(i)**2)! some sums
            Swx=Swx+w(i)*x(i)
            Swy=Swy+w(i)*y(i)
            Swxy=Swxy+w(i)*x(i)*y(i)
            Sw=Sw+w(i)
c           print *,'FIT LOOP:',sy(i),w(i),x(i),y(i),Swx2,Swx,Swy,Swxy,Sw
         ENDIF
      ENDDO

      delta=Sw*Swx2-(Swx**2)
      a=(Swx2*Swy-Swx*Swxy)/delta ! offset
      b=(Sw*Swxy-Swx*Swy)/delta ! slope
      ea=sqrt(Swx2/delta) ! offset standard deviation
      eb=sqrt(Sw/delta) ! slope standard deviation
      
      chi2=0.0
      DO i=1,N
         IF (y(i).GT.-97.) THEN
            chi2=chi2+((a+b*x(i))-y(i))**2/sy(i)**2
         ENDIF
      ENDDO

c      print *,'FIT RESULT:',a,b,chi2,Nfit
      RETURN
      END

C---------------------------------------------------------------------------
      REAL FUNCTION ENCORR(X)
C---------------------------------------------------------------------------

      REAL FFUN
      PARAMETER (CALIB=0.0001059994) 
      PARAMETER (P1=.8993962E-2)
      PARAMETER (P2=-.449717)
      PARAMETER (P3=10.90797)
      PARAMETER (P4=-.6768349E-2)


      FFUN = P1 + P4 * ATAN((X - P3) * P2)

      IF (FFUN.EQ.0.) THEN 
         FFUN = 1.E-10
      ENDIF   

      ENCORR = FFUN/CALIB 

      RETURN
      END

C---------------------------------------------------------------------------
      REAL FUNCTION CORRANG(X,ANGX)
C---------------------------------------------------------------------------

      REAL A,B,X,ANGX

      A = -0.017695 + ANGX * 0.0016963
      B = -0.049583 + ANGX * 0.0050639
      CORRANG = 0.
      IF (X.LE..9) CORRANG = A * (X - .9)
      IF (X.GE..9.AND.X.LE.1.1) CORRANG = 0.
      IF (X.GE.1.1) CORRANG = B * (X - 1.1)
      IF (ANGX.LT.10.) CORRANG = 0.

      RETURN
      END

