      program EIGEMIT
!
! Calculate eigenemittances of a particle distribution
!
      implicit real(8) (a-h,o-z) 
      include 'eigemit.inc'
      parameter ( version = 1.02 )
      dimension xp(3),pp(3),bfld(3),efld(3),pol(3)
      dimension mass(5)
      real(8) mass
      logical refp
      namelist/PAR/accx,accy,accz,canon,fname,itype,pacorr,pzmin,  &
        pzmax,rffreq,sigs,title,wracc
      data mass/0.510998906d-3, 0.10565837, 0.13957018, 0.493677,  &
        0.93827201/  ! GeV/c^2
!
      write(*,'(a,f5.2)') 'Welcome to program EIGEMIT, version ',  & 
        version
!
! Default parameters
      accx = 15d-3
      accy = 15d-3
      accz = 150d-3
      canon = .false.
      itype = 2
      pacorr = .false.
      pzmin = 0.100
      pzmax = 0.300
      rffreq = 201d0
      sigs = 4.
      title = ' '
      fname = ' '
      wracc = .false.
!
      do i=1,6          ! symplectic matrix
      do j=1,6
      jj(i,j) = 0d0
      end do
      end do
      jj(1,2) = 1d0
      jj(2,1) = -1d0
      jj(3,4) = 1d0
      jj(4,3) = -1d0
      jj(5,6) = 1d0
      jj(6,5) = -1d0
!
      open(unit=8,file='eigemit.inp',status='old',iostat=ioc)
      open(unit=10,file='eigemit.dat',status='unknown',  &
        recl=1024,iostat=ioc)
      open(unit=12,file='eigemit.aux',status='unknown',  &
        recl=1024,iostat=ioc)
!
      read(8,par,iostat=ioc)        ! read input parameters
      if( fname == ' ' ) fname = 'for009.dat'
      open(unit=9,file=fname,status='old',iostat=ioc)
      acc(1) = accx
      acc(2) = accy
      acc(3) = accz
      if( wracc ) then
        open(unit=14,file='eigemit.acc',status='unknown')
      end if
!
      write(10,'(a,f5.2)') 'Welcome to program EIGEMIT, version ',  & 
        version
      write(10,par)
      write(10,50)      ! emittance file header labels
50    format(t3,'reg',t10,'z',t17,'em1',t29,'em2',t41,'em3',  &
        t53,'em6e',t65,'wtsum',t77,'wtcut',t89,'wtamp',t101,'emX',  &
        t113,'emY',t125,'emZ',t137,'em4s',t149,'em6s',  &
        t161,'emT',t173,'emL')
      write(10,55)      ! emittance file header units
55    format(t3,' ',t9,'[m]',t17,'[m]',t29,'[m]',t41,'[m]',  &
        t52,'[m^3]',t65,' ',t77,' ',t89,' ',t101,'[m]',  &
        t113,'[m]',t125,'[m]',t136,'[m^2]',t148,'[m^3]',  &
        t161,'[m]',t173,'[m]')
!
      write(12,'(a,f5.2)') 'Welcome to program EIGEMIT, version ',  & 
        version
      write(12,'(a80)') title
      write(12,60)      ! auxiliary file header labels
60    format(t3,'reg',t10,'z',t19,'Pz',t28,'sgX',t40,'sgPx',  &
        t52,'sgY',t64,'sgPy',t76,'sgcT',t88,'sgE',t100,'alX',  &
        t109,'btX',t118,'gmX',t127,'alY',t136,'btY',t145,'gmY',  &
        t154,'alZ',t163,'btZ',t172,'gmZ',t181,'Bz',t190,'Lz',  &
        t199,'rEA',t208,'Dx',t217,'Dpx',t226,'Dy',t235,'Dpy')
      write(12,65)      ! auxiliary file header units
65    format(t3,' ',t9,'[m]',t16,'[GeV/c]',t28,'[m]',  &
        t39,'[GeV/c]',t52,'[m]',t63,'[GeV/c]',t76,'[m]',  &
        t87,'[GeV]',t98,' ',t108,'[m]',t117,'[1/m]',t125,' ',  &
        t135,'[m]',t144,'[1/m]',t152,' ',t162,'[m]',  &
        t171,'[1/m]',t181,'[T]',t187,'[m GeV/c]',t200,' ',  &
        t208,'[m]',t218,' ',t226,'[m]',t236,' ')
!
      read(9,*)      ! skip input header records
      read(9,*)
      read(9,*)
      read(9,*,iostat=ioc)ievt,ipnum,iptyp,ipflg,jsrg,  &	
        tp,xp,pp,bfld,evtwt,efld,sarc,pol
      if( ioc .ne. 0 ) go to 900
      jsold = jsrg
      zold = xp(3)
      backspace (unit=9)
      nrec = 0
      knt = 0
      wtsum = 0d0
      lastreg = .false.
      refp = .false.
! .........................................................
100   continue
      nrec = nrec + 1
      read(9,*,iostat=ioc)ievt,ipnum,iptyp,ipflg,jsrg,  &	
        tp,xp,pp,bfld,evtwt,efld,sarc,pol
      if( ioc .gt. 0 ) then
        write(*,'(a,i10)') 'Error reading (9) record ',nrec
        go to 900
      else if( ioc .eq. -1 ) then       ! EOF
        go to 300
      end if
!
      if( jsrg == jsold ) then
        if( ievt == 0 ) then
          tref = tp
          refp = .true.
        else
          if( ipflg .ne. 0 ) go to 100
          if( iptyp .ne. itype ) go to 100
          knt = knt + 1
          if( knt .gt. mxt ) then
            knt = mxt
            go to 100
          end if
          wtsum = wtsum + evtwt
          if( pp(3) .lt. pzmin ) go to 100
          if( pp(3) .gt. pzmax ) go to 100
!
          do i=1,3
          xpa(i,knt) = xp(i)
          ppa(i,knt) = pp(i)
          end do
          ta(knt) = tp
          pmass = mass( ABS(itype) )
          charge = SIGN( 1., REAL(iptyp) )
          pm2 = pp(1)**2 + pp(2)**2 + pp(3)**2
          ea(knt) = SQRT( pm2 + pmass**2 )
          wta(knt) = evtwt
          bza(knt) = bfld(3)
          ievta(knt) = ievt
          ipnuma(knt) = ipnum
          jsrga(knt) = jsrg
        end if
!
      else              ! region changed
        nt = knt
        call SETUP        ! Prepare input data for EIGEM
        call EIGEM        ! Compute eigenemittances
        call OUTPUT
        jsold = jsrg
        zold = xp(3)
        knt = 0
        wtsum = 0d0
        backspace (unit=9)
      end if
!
      go to 100
! .........................................................
300   continue
! Process last region
      lastreg = .true.
      if( wracc .and. refp ) then
        write(14,'(3i7)') 0,0,jsold
      end if
      call SETUP        ! Prepare input data for EIGEM
      call EIGEM        ! Compute eigenemittances
      call OUTPUT
!
900   continue
      end
! *********************************************************
      SUBROUTINE balanc(a,n,np)
      INTEGER n,np
!      REAL a(np,np),RADIX,SQRDX
      REAL*8 a(np,np),RADIX,SQRDX
      PARAMETER (RADIX=2.,SQRDX=RADIX**2)
      INTEGER i,j,last
      REAL c,f,g,r,s
1     continue
        last=1
        do 14 i=1,n
          c=0.
          r=0.
          do 11 j=1,n
            if(j.ne.i)then
              c=c+abs(a(j,i))
              r=r+abs(a(i,j))
            endif
11        continue
          if(c.ne.0..and.r.ne.0.)then
            g=r/RADIX
            f=1.
            s=c+r
2           if(c.lt.g)then
              f=f*RADIX
              c=c*SQRDX
            goto 2
            endif
            g=r*RADIX
3           if(c.gt.g)then
              f=f/RADIX
              c=c/SQRDX
            goto 3
            endif
            if((c+r)/f.lt.0.95*s)then
              last=0
              g=1./f
              do 12 j=1,n
                a(i,j)=a(i,j)*g
12            continue
              do 13 j=1,n
                a(j,i)=a(j,i)*f
13            continue
            endif
          endif
14      continue
      if(last.eq.0)goto 1
      return
      END
! *********************************************************
      subroutine EIGEM
!
! Calculate eigenemittances
!  Ref. Rangarajan, Neri & Dragt, Proc. PAC89, p. 1280.
!  Ref. Neri & Rangarajan PRL 64:1073, 1990.
! Arrays use variables {x,px,y,py,ct,-E,pz} 
!
      implicit real(8) (a-h,o-z) 
      include 'eigemit.inc'
!
      dimension cv3(3,3),cv4(4,4),cv6(6,6)
      dimension sj(6,6)
      dimension wr(6),wi(6)
      dimension indx(6)
!
      if( ntrc == 0 ) go to 900
!
! Compute eigen matrix [SJ] = [cov] [JJ] 
      do i=1,6
      do j=1,6
      sj(i,j) = 0d0
      end do
      end do
!
      do i=1,6
      do j=1,6
      do k=1,6
      sj(i,j) = sj(i,j) + cov(i,k)*jj(k,j)
      end do
      end do
      end do
! Get eigenvalues of the matrix
      call BALANC(sj,6,6)     ! balance the matrix  (NR)
      call ELMHES(sj,6,6)     ! reduce to Hessenberg form  (NR)
      call HQR(sj,6,6,wi,wr)  ! find eigenvalues  (NR)
!
      do i=1,3
      arg = wr(2*i-1)**2 + wi(2*i-1)**2
      em(i) = SQRT( arg )
      end do
      em(1) = em(1) / pmass
      em(2) = em(2) / pmass
      em(3) = em(3) / pmass
      e6e = em(1) * em(2) * em(3)
!
! Cartesian 2D rms sigma emittances
      arg = cov(1,1)*cov(2,2) - cov(1,2)*cov(2,1)
      emx = SQRT(arg) / pmass
      arg = cov(3,3)*cov(4,4) - cov(3,4)*cov(4,3)
      emy = SQRT(arg) / pmass
      arg = cov(5,5)*cov(6,6) - cov(5,6)*cov(6,5)
      if( arg .ge. 0d0 ) then
        emz = SQRT(arg) / pmass   
      else
        emz = vsd(5) * vsd(6) / pmass
      end if
!
! Compute 4D and transverse sigma emittances
      do i=1,4
      do j=1,4
      cv4(i,j) = cov(i,j)
      end do
      end do
      call LUDCMP(cv4,4,4,indx,e4s)  ! LU decomposition (NR)
      do i=1,4
      e4s = e4s * cv4(i,i)           ! det(cv4)
      end do
      if( e4s .gt. 0d0 ) then
        e4s = SQRT(e4s) / pmass**2
        emt = SQRT( e4s )
      else
        e4s = -1.
        emt = -1.
      end if
!
! Compute 6D sigma emittance
      do i=1,6
      do j=1,6
      cv6(i,j) = cov(i,j)
      end do
      end do
      call LUDCMP(cv6,6,6,indx,e6s)  ! LU decomposition (NR)
      do i=1,6
      e6s = e6s * cv6(i,i)           ! det(cv6)
      end do
      e6s = ABS(e6s)
!      if( e6s .gt. 0d0 ) then
        e6s = SQRT(e6s) / pmass**3
!      else
!        e6s = -1.
!      end if
!
! Find weighted average values of some useful quantities
      bz = 0d0
      amz = 0d0
!
      do i=1,ntrk
      if( flg(i) .ne. 0 ) cycle
      bz = bz + bza(i)*wta(i)
      angmom = xpa(1,i)*ppa(2,1) - xpa(2,1)*ppa(1,i)
      amz = amz + angmom*wta(i)   
      end do
!
      bz = bz / wtcut         ! for output information
      amz = amz / wtcut
!
      e0 = -vmn(6)
      pz = vmn(7)
      beta = pz / e0
      bg = pz / pmass
!
! Dispersions
      disp(1) = cov(1,7) * pz / cov(7,7)
      disp(2) = cov(2,7) / cov(7,7)
      disp(3) = cov(3,7) * pz / cov(7,7)
      disp(4) = cov(4,7) / cov(7,7)
!
! Compute transverse Courant-Snyder parameters
      alcs(1) = -cov(1,2) / (emx * pmass)
      btcs(1) = cov(1,1) * pz / (emx * pmass)
      gmcs(1) = cov(2,2) / (emx * pz * pmass)
      alcs(2) = -cov(3,4) / (emy * pmass)
      btcs(2) = cov(3,3) * pz / (emy * pmass)
      gmcs(2) = cov(4,4) / (emy * pz * pmass)
!
! Energy - transverse amplitude correlation
      do i=1,3        ! use variables {ct, E, At}
      do j=1,3
      cv3(i,j) = 0d0
      end do
      end do
      atavg = 0d0
!
      do i=1,ntrk
      if( flg(i) .ne. 0 ) cycle
      csix = bg*btcs(1)*(ppa(1,i)/ppa(3,i))**2  &
        + 2d0*bg*alcs(1)*xpa(1,i)*ppa(1,i)/ppa(3,i)  &
        + bg*gmcs(1)*xpa(1,i)**2
      csiy = bg*btcs(2)*(ppa(2,i)/ppa(3,i))**2  &
        + 2d0*bg*alcs(2)*xpa(2,i)*ppa(2,i)/ppa(3,i)  &
        + bg*gmcs(2)*xpa(2,i)**2
      at = SQRT( csix**2 + csiy**2 ) 
      cv3(1,3) = cv3(1,3) + ta(i)*at*wta(i)
      cv3(2,3) = cv3(2,3) + ea(i)*at*wta(i)
      cv3(3,3) = cv3(3,3) + at*at*wta(i)
      atavg = atavg + at*wta(i)
      end do
!
      atavg = atavg / wtcut
      cv3(1,3) = cv3(1,3)/wtcut - vmn(5)*atavg
      cv3(2,3) = cv3(2,3)/wtcut - ABS(vmn(6))*atavg
      cv3(3,3) = cv3(3,3)/wtcut - atavg*atavg
      cv3(3,1) = cv3(1,3)
      cv3(3,2) = cv3(2,3)
      cv3(1,1) = cov(5,5)
      cv3(1,2) = cov(5,6)
      cv3(2,1) = cov(6,5)
      cv3(2,2) = cov(6,6)
      rea = cv3(2,3) / ( vsd(6)*SQRT(cv3(3,3)) )   ! correlation
!
! Longitudinal quantities depending on the E-At correlation
      if( pacorr ) then       ! apply momentum-amplitude corrections
!
        call LUDCMP(cv3,3,3,indx,eml)  ! LU decomposition (NR)
        do i=1,3
        eml = eml * cv3(i,i)           ! det(cv3)
        end do
        arg = ABS(eml) / cv3(3,3)
!        if( arg .gt. 0d0 ) then     ! longitudinal sigma emittance
          eml = SQRT(arg) / pmass
!        else
!          eml = -1.
!        end if
!  
        th = 0.5d0*ATAN(2d0*cv3(2,3)) / (cv3(3,3)-cv3(2,2))
        sth = SIN(th)
        cth = COS(th)
        arg = cv3(2,2)*cth**2 - 2d0*sth*cth*cv3(2,3)  &
          + cv3(3,3)*sth**2
        vsd(6) = SQRT(arg)                ! corrected energy spread
        if( eml .gt. 0d0 ) then
          vsd(5) = pmass*eml / vsd(6)       ! corrected ct spread
          alcs(3) = -cv3(1,2) / (eml*pmass)      
          btcs(3) = cv3(1,1)*pz*beta**2 / (eml*pmass)
          gmcs(3) = cv3(2,2)*pz / (eml*pmass*e0*e0*beta**4) 
        else
          vsd(5) = -1.
          alcs(3) = -1.
          btcs(3) = -1.
          gmcs(3) = -1.
        end if
!
      else                    ! no corrections
        if( e6s .gt. 0d0 ) then
          eml = e6s / e4s 
        else
          eml = -1.
        end if
        alcs(3) = -cov(5,6) / (emz*pmass)      
        btcs(3) = cov(5,5)*pz*beta**2 / (emz*pmass)
        gmcs(3) = cov(6,6)*pz / (emz*pmass*e0*e0*beta**4) 
      end if            ! end of test on pacorr
!
! Find number of particles inside the acceptance
      wtamp = 0d0
      do i=1,ntrk
      if( flg(i) .ne. 0 ) cycle
      csix = bg*btcs(1)*(ppa(1,i)/ppa(3,i))**2  &
        + 2d0*bg*alcs(1)*xpa(1,i)*ppa(1,i)/ppa(3,i)  &
        + bg*gmcs(1)*xpa(1,i)**2
      csiy = bg*btcs(2)*(ppa(2,i)/ppa(3,i))**2  &
        + 2d0*bg*alcs(2)*xpa(2,i)*ppa(2,i)/ppa(3,i)  &
        + bg*gmcs(2)*xpa(2,i)**2
      if( csix.gt.acc(1) .or. csiy.gt.acc(2) ) cycle
      fe = (ea(i) - e0) / e0
      csiz = bg*btcs(3)*fe**2 / beta**4  &
        + 2d0*bg*alcs(3)*fe*ta(i) / beta  &
        + bg*gmcs(3)*beta**2*ta(i)**2
      if( csiz .gt. acc(3) ) cycle
      wtamp = wtamp + wta(i)
      if( wracc .and. lastreg ) then
        write(14,'(3i7)') ievta(i),ipnuma(i),jsrga(i)
      end if
      end do
!
900   continue
      end
! *********************************************************
      SUBROUTINE elmhes(a,n,np)
      INTEGER n,np
!      REAL a(np,np)
      REAL*8 a(np,np)
      INTEGER i,j,m
      REAL x,y
      do 17 m=2,n-1
        x=0.
        i=m
        do 11 j=m,n
          if(abs(a(j,m-1)).gt.abs(x))then
            x=a(j,m-1)
            i=j
          endif
11      continue
        if(i.ne.m)then
          do 12 j=m-1,n
            y=a(i,j)
            a(i,j)=a(m,j)
            a(m,j)=y
12        continue
          do 13 j=1,n
            y=a(j,i)
            a(j,i)=a(j,m)
            a(j,m)=y
13        continue
        endif
        if(x.ne.0.)then
          do 16 i=m+1,n
            y=a(i,m-1)
            if(y.ne.0.)then
              y=y/x
              a(i,m-1)=y
              do 14 j=m,n
                a(i,j)=a(i,j)-y*a(m,j)
14            continue
              do 15 j=1,n
                a(j,m)=a(j,m)+y*a(j,i)
15            continue
            endif
16        continue
        endif
17    continue
      return
      END
! *********************************************************
      SUBROUTINE hqr(a,n,np,wr,wi)
      INTEGER n,np
!      REAL a(np,np),wi(np),wr(np)
      REAL*8 a(np,np),wi(np),wr(np)
      INTEGER i,its,j,k,l,m,nn
      REAL anorm,p,q,r,s,t,u,v,w,x,y,z
      anorm=0.
      do 12 i=1,n
        do 11 j=max(i-1,1),n
          anorm=anorm+abs(a(i,j))
11      continue
12    continue
      nn=n
      t=0.
1     if(nn.ge.1)then
        its=0
2       do 13 l=nn,2,-1
          s=abs(a(l-1,l-1))+abs(a(l,l))
          if(s.eq.0.)s=anorm
          if(abs(a(l,l-1))+s.eq.s)goto 3
13      continue
        l=1
3       x=a(nn,nn)
        if(l.eq.nn)then
          wr(nn)=x+t
          wi(nn)=0.
          nn=nn-1
        else
          y=a(nn-1,nn-1)
          w=a(nn,nn-1)*a(nn-1,nn)
          if(l.eq.nn-1)then
            p=0.5*(y-x)
            q=p**2+w
            z=sqrt(abs(q))
            x=x+t
            if(q.ge.0.)then
              z=p+sign(z,p)
              wr(nn)=x+z
              wr(nn-1)=wr(nn)
              if(z.ne.0.)wr(nn)=x-w/z
              wi(nn)=0.
              wi(nn-1)=0.
            else
              wr(nn)=x+p
              wr(nn-1)=wr(nn)
              wi(nn)=z
              wi(nn-1)=-z
            endif
            nn=nn-2
          else
            if(its.eq.30)pause 'too many iterations in hqr'
            if(its.eq.10.or.its.eq.20)then
              t=t+x
              do 14 i=1,nn
                a(i,i)=a(i,i)-x
14            continue
              s=abs(a(nn,nn-1))+abs(a(nn-1,nn-2))
              x=0.75*s
              y=x
              w=-0.4375*s**2
            endif
            its=its+1
            do 15 m=nn-2,l,-1
              z=a(m,m)
              r=x-z
              s=y-z
              p=(r*s-w)/a(m+1,m)+a(m,m+1)
              q=a(m+1,m+1)-z-r-s
              r=a(m+2,m+1)
              s=abs(p)+abs(q)+abs(r)
              p=p/s
              q=q/s
              r=r/s
              if(m.eq.l)goto 4
              u=abs(a(m,m-1))*(abs(q)+abs(r))
              v=abs(p)*(abs(a(m-1,m-1))+abs(z)+abs(a(m+1,m+1)))
              if(u+v.eq.v)goto 4
15          continue
4           do 16 i=m+2,nn
              a(i,i-2)=0.
              if (i.ne.m+2) a(i,i-3)=0.
16          continue
            do 19 k=m,nn-1
              if(k.ne.m)then
                p=a(k,k-1)
                q=a(k+1,k-1)
                r=0.
                if(k.ne.nn-1)r=a(k+2,k-1)
                x=abs(p)+abs(q)+abs(r)
                if(x.ne.0.)then
                  p=p/x
                  q=q/x
                  r=r/x
                endif
              endif
              s=sign(sqrt(p**2+q**2+r**2),p)
              if(s.ne.0.)then
                if(k.eq.m)then
                  if(l.ne.m)a(k,k-1)=-a(k,k-1)
                else
                  a(k,k-1)=-s*x
                endif
                p=p+s
                x=p/s
                y=q/s
                z=r/s
                q=q/p
                r=r/p
                do 17 j=k,nn
                  p=a(k,j)+q*a(k+1,j)
                  if(k.ne.nn-1)then
                    p=p+r*a(k+2,j)
                    a(k+2,j)=a(k+2,j)-p*z
                  endif
                  a(k+1,j)=a(k+1,j)-p*y
                  a(k,j)=a(k,j)-p*x
17              continue
                do 18 i=l,min(nn,k+3)
                  p=x*a(i,k)+y*a(i,k+1)
                  if(k.ne.nn-1)then
                    p=p+z*a(i,k+2)
                    a(i,k+2)=a(i,k+2)-p*r
                  endif
                  a(i,k+1)=a(i,k+1)-p*q
                  a(i,k)=a(i,k)-p
18              continue
              endif
19          continue
            goto 2
          endif
        endif
      goto 1
      endif
      return
      END
! *********************************************************
      SUBROUTINE ludcmp(a,n,np,indx,d)
      INTEGER n,np,indx(n),NMAX
!      REAL d,a(np,np),TINY
      REAL*8 d,a(np,np),TINY
      PARAMETER (NMAX=500,TINY=1.0e-20)
      INTEGER i,imax,j,k
      REAL aamax,dum,sum,vv(NMAX)
      d=1.
      do 12 i=1,n
        aamax=0.
        do 11 j=1,n
          if (abs(a(i,j)).gt.aamax) aamax=abs(a(i,j))
11      continue
        if (aamax.eq.0.) pause 'singular matrix in ludcmp'
        vv(i)=1./aamax
12    continue
      do 19 j=1,n
        do 14 i=1,j-1
          sum=a(i,j)
          do 13 k=1,i-1
            sum=sum-a(i,k)*a(k,j)
13        continue
          a(i,j)=sum
14      continue
        aamax=0.
        do 16 i=j,n
          sum=a(i,j)
          do 15 k=1,j-1
            sum=sum-a(i,k)*a(k,j)
15        continue
          a(i,j)=sum
          dum=vv(i)*abs(sum)
          if (dum.ge.aamax) then
            imax=i
            aamax=dum
          endif
16      continue
        if (j.ne.imax)then
          do 17 k=1,n
            dum=a(imax,k)
            a(imax,k)=a(j,k)
            a(j,k)=dum
17        continue
          d=-d
          vv(imax)=vv(j)
        endif
        indx(j)=imax
        if(a(j,j).eq.0.)a(j,j)=TINY
        if(j.ne.n)then
          dum=1./a(j,j)
          do 18 i=j+1,n
            a(i,j)=a(i,j)*dum
18        continue
        endif
19    continue
      return
      END
! *********************************************************
      subroutine MEANSD(v,wt,ntrk,vmn,vsd,ierr)
!
! Find mean and standard deviation of array variables
!
      implicit real(8) (a-h,o-z) 
      dimension v(7,ntrk),wt(ntrk),vmn(7),vsd(7)
!
      ierr = 0
      do i=1,7
      vmn(i) = 0d0
      vsd(i) = 0d0
      end do
      wts = 0d0
!
      if( ntrk .le. 0 ) then
        ierr = 1
        go to 900
      end if
!
      do i=1,ntrk
      wts = wts + wt(i)
      do j=1,7
      vmn(j) = vmn(j) + v(j,i)*wt(i)
      vsd(j) = vsd(j) + v(j,i)*v(j,i)*wt(i)
      end do
      end do
!
      do i=1,7
      vmn(i) = vmn(i) / wts
      vsd(i) = vsd(i) / wts        ! 2nd moment
      end do  
!
      do i=1,7
      arg = vsd(i) - vmn(i)**2
      if( arg .ge. 0d0 ) then
        vsd(i) = SQRT( arg )
      else
        ierr =2
        go to 900
      end if
      end do          
!
900   continue
      end
! *********************************************************
      subroutine OUTPUT
!
! Write out emittances for this location
!
      implicit real(8) (a-h,o-z) 
      include 'eigemit.inc'
!
      write(10,50) jsold,zold,em,e6e,wtsum,wtcut,wtamp,emx,emy,  &
        emz,e4s,e6s,emt,eml
50    format(i4,f8.3,1pe12.4,3e12.4,3e12.4,7e12.4,0pf8.3,f8.3)
      write(12,60) jsold,zold,pz,(vsd(i),i=1,6),alcs(1),btcs(1),  &
        gmcs(1),alcs(2),btcs(2),gmcs(2),alcs(3),btcs(3),gmcs(3),  &
        bz,amz,rea,disp
60    format(i4,2f9.4,1pe12.4,5e12.4,0pf9.4,15f9.4)
      write(*,'(i4,f8.3,4e12.4)') jsold,zold,em,e6e
!
      end
! *********************************************************
      subroutine SETUP
!
! Load variable array V={x,px,y,py,ct,-E,pz} for fixed z
! Collapse bunch trains to single bucket
! Set FLG array from tail cutting logic
! Calculate beam covariance matrix
!
      implicit real(8) (a-h,o-z) 
      include 'eigemit.inc'
      data cc/299792458d0/
      data mxcy/10/
      data con/0.14989623d0/
!
! Load the variable array V
      ntrk = nt
      ntrc = nt
      do i=1,ntrk
      v(1,i) = xpa(1,i)
      v(3,i) = xpa(2,i)
      if( canon ) then
        v(2,i) = ppa(1,i) - con*xpa(2,i)*bza(i)
        v(4,i) = ppa(2,i) + con*xpa(1,i)*bza(i)
      else
        v(2,i) = ppa(1,i)
        v(4,i) = ppa(2,i)
      end if
      v(5,i) = cc * (ta(i) - tref)
      v(6,i) = -ea(i)
      v(7,i) = ppa(3,i)       
      flg(i) = 0
      end do
!
! Collapse bunch trains to single RF bucket
      if( rffreq .le. 0d0 ) go to 800
      per = 1d0 / (rffreq*1d6)            ! [s]
      do i=1,ntrk
      if( charge .lt. 0.d0 ) then
        ctoff = 0.5d0 * per * cc
      else
        ctoff = 0d0
      end if
      dct = v(5,i)
      count = (dct+ctoff)/(cc * per) - 0.5d0
      dct = dct - NINT(count) * per
      v(5,i) = dct
      end do
!
! Flag events with variables out in the tails
      knt = 0
100   continue
      knt = knt + 1
      if( knt .gt. mxcy ) then
        go to 200
      end if
!
      nold = ntrc
      call MEANSD(v,wta,ntrc,vmn,vsd,ierr)
      if( ierr .ne. 0 ) go to 800
!
      do i=1,ntrk
      if( flg(i) .ne. 0 ) cycle
      do j=1,6
      if( ABS(v(j,i)-vmn(j)) .gt. sigs*vsd(j) ) then
        flg(i) = j
        ntrc = ntrc - 1
        exit
      end if
      end do
      end do
!
      if( ntrc .ne. nold ) go to 100
!
! Calculate beam covariance matrix
200   continue
      if( ntrc == 0 ) go to 900
!
      do i=1,7
      do j=1,7
      cov(i,j) = 0d0
      end do
      end do
      wtcut = 0d0
!
      do i=1,ntrk
      if( flg(i) .ne. 0 ) cycle
      wtcut = wtcut + wta(i)
      do j=1,7
      do k=1,7
      cov(j,k) = cov(j,k) + v(j,i)*v(k,i)*wta(i)
      end do
      end do
      end do
!
      do i=1,7          
      do j=1,7
      cov(i,j) = cov(i,j)/wtcut - vmn(i)*vmn(j)
      end do
      end do
      go to 900
!
800   continue
      write(*,*) 'Error in SETUP'
      stop
!
900   continue
      end
