/*:>--------------------------------------------------------------------
**: FILE:       mMvdZcor.cc
**: HISTORY:
**:             00jan93-v000a-hpl- Created by stic Version
**:  Id: idl.y,v 1.17 1997/03/25 19:22:52 ward Exp  
**:<------------------------------------------------------------------*/
#include "mMvdZcor.h"
#include <math.h>
#include <stdlib.h>
#include <time.h>
#include "PhHistogramFactory.hh"
#include <iostream.h>


void expandBarrel(                       DMVDGEO_ST  *Geo,
                  TABLE_HEAD_ST *bRaw_h, DMVBRAW_ST *bRaw,
                                         DMVBPAR_ST *bPar,
                  int   ih_inner[][6],
                  int   ih_outer[][6],
                  short *lastindex    );

void compressOuterBarrel( DMVDPAR_ST  *Par, 
                          DMVBPAR_ST *bPar,
                          float ratio,
                          int   ih_comp[][6],
                          int   ih_outer[][6],
                          short nzexpd,
                          short *maxzout );

float  calculateRms( short ioff,
                     int ih_comp[][6],
                     int ih_inner[][6],
                     int nzbig,
                     int nzexpd,
                     int maxzout,
                     int NPH  );

#define SOFTWARE_VERSION 2.01
#define LOUD 3
#define VERY_LOUD 4

long mMvdZcor_(
  TABLE_HEAD_ST            *Bbc_h,        DBBCOUT_ST              *Bbc ,
  TABLE_HEAD_ST            *Geo_h,        DMVDGEO_ST              *Geo ,
  TABLE_HEAD_ST            *Par_h,        DMVDPAR_ST              *Par ,
  TABLE_HEAD_ST           *bGeo_h,        DMVBGEO_ST             *bGeo ,
  TABLE_HEAD_ST           *bPar_h,        DMVBPAR_ST             *bPar ,
  TABLE_HEAD_ST           *bRaw_h,        DMVBRAW_ST             *bRaw ,
  TABLE_HEAD_ST          *Cntrl_h, DMVDTRIGCONTROL_ST           *Cntrl ,
  TABLE_HEAD_ST      *VertexOut_h,  DMVDVERTEXOUT_ST        *VertexOut ,
  TABLE_HEAD_ST           *zPar_h,    DMVDZCORPAR_ST             *zPar ,
  TABLE_HEAD_ST             *Io_h,         DMVDIO_ST               *Io )
{
/*:>--------------------------------------------------------------------
**: ROUTINE    :    mMvdZcor_
**: DESCRIPTION: Code to find the vertex position using the
**:             "correlation" method. The technique is based on the
**:              idea that, when the pattern of hits on the outer
**:              detector is scaled down by a factor of the radii,
**:              the pattern of hits on the outer detector is
**:              the same as on the inner detector, with an offset
**:              in the z direction -- where the offset in the
**:              z direction is linearly related to z of the vertex.
**:              This algorithm does not find x and y of the vertex --
**:              they are just set to zero. The calculated z vertex
**:              position is VertexOutZcor->vertex[2]. The vertex
**:              position from the beam-beam counters is used to narrow
**:              the search to a range of +-5cm from the beam-beam
**:              counter vertex vertex, when it is known. When it is
**:              not known the whole central region is searched to find
**:              the vertex.
**:
**: AUTHOR     : J.H.Park, S.S.Ryu (YonSei Univ.) (Nov 10 97)
**:              Translated from the original Fortran version VER_ZCOR.f 
**:              written by J.P.Sullivan 
**:              The original idea came from Hubert van Hecke.
**:
**: ARGUMENTS:
**:       IN:
**:                Bbc    - contains vertex position from BBC
**:               Bbc_h   - header Structure for Bbc
**:                Geo    - MVD geometry parameters
**:               Geo_h   - header Structure for Geo
**:                Par    - misc parameters related to such
**:                         things as digitization
**:               Par_h   - header Structure for Par
**:               bGeo    - hold z of each strip and
**:                         each detector "cell".
**:              bGeo_h   - header Structure for bGeo
**:               bPar    - hold various key barrel dimensions,
**:                         like the number of phi segments.
**:              bPar_h   - header Structure for bPar
**:               bRaw    - raw data structure for barrel
**:              bRaw_h   - header Structure for bRaw
**:               Cntrl    - contains choice parameter, which controls
**:                         which vertex-finding algorithm is used
**:               Cntrl_h  - header structure for Cntrl
**:    INOUT:
**:          VertexOut    - vertex-finding output table
**:          VertexOut_h  - header structure for VertexOut
**:               zPar    - parameters related to the
**:                         correlation algorithm.
**:              zPar_h   - header Structure for zPar
**:      OUT:
**: RETURNS:    STAF Condition Value
**:    Revision History
**:  Name             Date            Comment
**:  J.H.Park         Feb. 10, 1998   controll execution of this module
**:                                   by the value of choice 
**:                                   narrow range of searching vertex
**:                                   by using dBbcOut table from BBC
**:  J.P.Sullivan     Apr.  6, 1998   add code to calculate and histogram
**:                                   cpu time used.
**:
**:  J.P.Sullivan     Apr. 14, 1998   replace printf calls with MvdMess_
**:  J.P.Sullivan     Jul. 29, 1998   remove used of STAFCV_BAD
**:  J.P>Sullivan     Jul. 31, 1998   switch to "C++" (actually no big
**:                                   changes in most of code), replace
**:                                   hbook calls with generic hist
**:                                   interface, find vertex using local
**:                                   arrays rather than hbook hists,
**:                                   replace mMvdMess with cout.
**:  J.P.Sullivan  13-Aug-98   Switch to "C++" (actually no big changes
**:                            in most of code), replace hbook calls 
**:                            with generic hist interface, find vertex
**:                            using local arrays rather than hbook
**:                            hists, replace mMvdMess with cout. Add
**:                            dMvdVertexOut table and the code
**:                            to fill it. Remove dMvdZcor table.
**:                            Replace mMvdMess calls with cout. Use
**:                            root histograms instead of hbook. Use 
**:                            local arrays for vertex finding instead
**:                            of histograms. Misc minor changes related
**:                            to type-casting.
**:  J.P.Sullivan  23-Sep-98   Add Io to calling sequence and use it
**:                            to control voliume of output.
**:  S.S.Ryu        06-Jul-99  changes the variable "Cntl" to "Cntrl" to make 
**:                            this variable consistent throughout the MVD 
**:                            codes.
**:>------------------------------------------------------------------*/


int ioff;
int i,j,ichmin,ioff_min,ioff_max,ioff_zmin,ioff_zmax;
int imin,imax;
int IEXPND;
float cputime,cpu_in,cpu_out;   /* for tests of cpu time use */

/* variables associated with "histogram" array to find vertex: */
float xval;
float *y_data;
float dxstep;
int nbin;
float x1,x2;
float argtmp;

/* array size =2*bPar->NZBIG+1=2*3200+1  */

short NZEXPD;
 /* array size = [bPar->NZBIG][bPar->NPHIDIM]=[3200][6] */
int ih_comp[3200][6];
int ih_inner[3200][6],ih_outer[3200][6];
float ymin,test1,test2,slope,bvalue,rms_zval,ximin,ximax;
float xmin;
float ratio;
float range;
short MAXZOUT;

int NumOut;
static int ncalls=0;

/* stuff for interface to generic histogram interface: */
static ifirst=0;
PhHistogramFactory *hf = PhHistogramFactory::instance();
static PhH1 *CorrZ;
static PhH1 *CpuPerEvC;
static PhH2 *CpuVsOccC;

float full_scale =Par->full_scale;

int MAXZDIM=2*bPar->NZBIG+1;
int NZBIG=bPar->NZBIG;

if ( Cntrl->choice ==2 )
   return STAFCV_OK;

cpu_in=(float)clock()/(float)CLOCKS_PER_SEC;
if ( Io->verbose < LOUD ) cout << "\nmMvdZcor {" << endl;

/* ***************************************************************** */
/* The first step will be to initialize (mostly zeroes) the contents */
/* of the output table.                                              */

ncalls++;
NumOut = VertexOut_h->nok;    /* This saves the number of the row we */
                              /* are currently filling               */
VertexOut_h->nok++;
for ( i=0 ; i<3 ; i++ ) {
  VertexOut[NumOut].vertex[i]=0.;
  for ( j=0 ; j<3 ; j++ )
     VertexOut[NumOut].vertexerr[i][j]=0.;
}
VertexOut[NumOut].softvertex=SOFTWARE_VERSION;
VertexOut[NumOut].ErrorCode=0;  /* 1 means OK, negative means error */
VertexOut[NumOut].vertexcl=1.;  /* use 1 until there is a better number */
VertexOut[NumOut].id=ncalls;    /* N event would be better              *
ErrorCode=0;
/* ******************************************************************** */

/*  setup parameters for Z correlation method */
if ( zPar_h->nok==0 ){    
   zPar->IEXPND=1;
   zPar_h->nok=1;
}

/* if B-B Counter would not work, we will expand the search 
range of vertex */

IEXPND=zPar->IEXPND; 

ratio=(Geo->vislr+Geo->dvisl[0])/
        (Geo->voslr+Geo->dvosl[0]);
range=(float)NZBIG+0.5;

x1=-range;
x2= range;

if ( ifirst==0 ) {
  ifirst=1;
  hf->cd();
  /* hf->mkdir("MVD"); */
  hf->cd("MVD");
  CpuPerEvC = hf->CreateH1F("CpuPerEvC","CPU sec/ev (correlation)",
                                   500, 0., 50.);
  CpuVsOccC = hf->CreateH2F("CpuVsOccC","CPU sec/ev vs. occ. (corr.)",
                            20,0.,1.,100,0.,100.);
  CorrZ = hf->CreateH1F("CorrZ","hist for Z vertex",MAXZDIM,x1,x2);
}
else {
  hf->cd();
  hf->cd("MVD");
  CorrZ->Reset();
}

y_data=(float *)calloc(MAXZDIM,sizeof(float));
if ( y_data==NULL ) {
   if ( Io->verbose < VERY_LOUD ) cout <<"\t" << " ERROR: out of memory ================= " << endl;

   VertexOut[NumOut].ErrorCode=-1;  /* no memory for vertex search */

   return STAFCV_OK;  /* this should really be STAFCV_BAD, but it */
                      /* causes problems on the linux machines    */
}

dxstep=(x2-x1)/(float)MAXZDIM;
for ( i=0 ; i<MAXZDIM ; i++ )
    y_data[i]=0;

expandBarrel(        Geo,
             bRaw_h,bRaw,
                    bPar,
                ih_inner,
                ih_outer,
                 &NZEXPD );

compressOuterBarrel(  Par,
                     bPar,
                    ratio,
                  ih_comp,
                 ih_outer, 
                   NZEXPD,
                 &MAXZOUT );


/* min and max offset values are calculated 2 ways:
a) ioff_min and ioff_max are the limits which correspond to no
   further overlap between the inner and outer arrays
b) ioff_zmin and ioff_zmax are the values of the offset corresponding
   to the min and max values of z to be considered in the vertex
   search -- in general these limits are more stringent. In order
   to search over the whole range of possible vertices, the min and
   max values of z to be considered are +-IEXPND*ZOFF. Notice that
   The offset value associated with Zmin(ioff_zmin) is LARGER than
   the offset value associated with Zmax(ioff_zmax). An alternative
   is to use the real vertex to limit the range of vertex positions
   searched to be +-5cm from the real vertex (assume the beam-beam
   counters will give the vertex at this accuracy).
c) In the process, the (linear) relationship between the offset in
   the histogram comparison and the vertex z position is determined.
   Zvertex = SLOPE*IOFF + bvalue */

ioff_min=1-NZEXPD;
ioff_max=MAXZOUT-1;

bvalue=bGeo->zstrip[0];
slope =(bGeo->zstrip[bPar->NZDIM-1]-bvalue)/((ratio-1.0)*(float)NZEXPD);

/* if beam-beam counters  would not fill dBbcOut table */
if ( Bbc_h->nok==0) {
 ioff_zmin=(int)((-(float)IEXPND*Par->zoff-bvalue)/slope);
 ioff_zmax=(int)(( (float)IEXPND*Par->zoff-bvalue)/slope);  
}

/* the estimate from the beam-beam counters should be used.
Narrow the search range to +-5cm from the real vertex. */

if ( Bbc_h->nok==1) {
ioff_zmin=(int)((Bbc->VertexPoint-Geo->ver_z_center-5.0-bvalue)/slope-1);
ioff_zmax=(int)((Bbc->VertexPoint-Geo->ver_z_center+5.0-bvalue)/slope-1); 
 }

imax=ioff_zmin<ioff_max? ioff_zmin:ioff_max;
imin=ioff_zmax>ioff_min? ioff_zmax:ioff_min;

if ( imin > imax) {
         if ( Io->verbose < VERY_LOUD )
           cout << "\tzCorrelation Vertex finding ERROR:"
                << " imin=" << imin
                << " imax=" << imax << endl;

         VertexOut[NumOut].ErrorCode=-2;  /* min chan for search is
                                            greater than max ? */
         free ( y_data);

         /* do not do cpu time calculation for this case where the */
         /* algorithm failed and most of it was never done         */

         if ( Io->verbose < LOUD ) cout << "}" << endl;

         return STAFCV_OK;  /* remove use of STAFCV_BAD because it */
                            /* causes a crash on rcf linux machines*/
}

/*  Now, calculate the rms differences between the histograms as a function
of the offset -- put the result in a histogram  */

for (ioff=imin;ioff<=imax;++ioff){
   rms_zval= calculateRms(   ioff,
                          ih_comp,
                         ih_inner,
                            NZBIG,
                           NZEXPD,
                          MAXZOUT,
                     bPar->NPHUSE );

   nbin = (int)(((float)ioff-x1)/dxstep);
   if ( nbin>0 && nbin<MAXZDIM )
       y_data[nbin]+=rms_zval;

}

/* Now, a histogram of the rms difference between the inner and outer
histograms as a function of the offset has been calculated.
Find the minimum value in this histogram and assume that that is close 
to the vertex.
YMIN is the minimum value found in the histogram ICHMIN is the channel
number in the histogram at which the min was found.
The loop below is over all channels in this histogram.
A more clever algorithm could only loop over the channel range IMIN -
IMAX, but in order to avoid possible errors in the use of offsets, 
this version does not take advantage of that possibility. */

ximin=(float)imin,ximax=(float)imax;
ichmin=0;
ymin=full_scale;

for (i=0;i< MAXZDIM;++i) {
    xval = x1 + ((float)i+0.5)*dxstep;
   if (xval > ximin && xval < ximax ){
      if (y_data[i] < ymin) {
             ymin=y_data[i];
             ichmin=i;
             xmin=xval;
      }

      /* this slightly strange looking section considers
      the case where two channels have the same value and
      that value is the min (so far anyway).  Ignoring
      the case where there are no counts, assume that
      the one closest to the center of the detector is
      more likely to be correct since that should be
      where the vertex distribution is centered -- we hope
      this does not happen much!  */

      if ( y_data[i] ==ymin && y_data[i] >0 ) {
          argtmp = slope*xmin+bvalue;
          test1 = (argtmp > 0) ? argtmp : -argtmp; /* test1 = abs(argtmp) */
          argtmp = slope*xval+bvalue;
          test2 = (argtmp > 0) ? argtmp : -argtmp; /* test2 = abs(argtmp) */
          if ( test2 < test1) {
             ymin=y_data[i];
             ichmin=i;
             xmin=xval;
          }
      }
   }
}

if ( ymin >0 ) {
   VertexOut[NumOut].ErrorCode= 1;  /* means all is OK */
   VertexOut[NumOut].vertex[2]= slope*xmin + bvalue+Geo->ver_z_center;

/* fill diagnostic histogram then free the memory used */
   for (i=0;i< MAXZDIM;++i) {
     xval = x1 + ((float)i+0.5)*dxstep;
     CorrZ->Fill(xval,y_data[i]);
   }
   free ( y_data);

   /*get final cpu time*/
   cpu_out=(float)clock()/(float)CLOCKS_PER_SEC;
   cputime=cpu_out - cpu_in;

   
   if ( Io->verbose < LOUD ) {
     cout << "\tzCorrelation method : Zvertex = " 
          << VertexOut[NumOut].vertex[2] << endl;
     cout << "\tCPU time elapsed=" << cputime << endl;
     cout << "}" << endl;
   }

   CpuPerEvC->Fill(cputime);
   CpuVsOccC->Fill(Cntrl[0].foccup[0],cputime);

   return STAFCV_OK;
}
else  {
   VertexOut[NumOut].ErrorCode=-3;  /* there seems to be no data */
   free ( y_data );

   /* do not do the cpu time calculation for a case like */
   /* this where nothing worked anyway                   */

   if ( Io->verbose < VERY_LOUD ) {
     cout << "\tzCorrelation method ERROR!; ymin=<0" << endl;
     cout << "\n}" << endl ;
   }

   return STAFCV_OK;  /* remove use of STAFCV_BAD because it */
                      /* causes a crash on rcf linux machines*/
}

}

void expandBarrel(                       DMVDGEO_ST  *Geo,
                  TABLE_HEAD_ST *bRaw_h, DMVBRAW_ST *bRaw,
                                         DMVBPAR_ST *bPar,
                  int ih_inner[][6],
                  int ih_outer[][6],
                  short *lastindex ){

   /* Expand the inner and outer barrel hits array for use with 
   the correlation nethod of vertex finding. Put "-1" in artificial 
   channels where there are dead spots */
 
   int i,ii,j,jj,k,kk;
   int n_diff,nstrip,nwperp;
   int nzbig=bPar->NZBIG; 
   float dz_active,dz_diff;

   *lastindex=0;

   nstrip=Geo->nstrip;
   nwperp=Geo->nwperp;
   dz_active=nstrip*Geo->pitch;
   dz_diff=(2.0*Geo->dvwr1[2])-dz_active;
   n_diff=(float)(dz_diff/Geo->pitch)-(int)(dz_diff/Geo->pitch)>0.5?
          (int)(dz_diff/Geo->pitch)+1 : (int)(dz_diff/Geo->pitch);
   *lastindex=nstrip*nwperp+n_diff*(nwperp-1); 

   for (i=0;i<*lastindex;++i)
   for (j=0; j< bPar->NPHUSE;++j) {
      ih_inner[i][j]=0;
      ih_outer[i][j]=0;
   }

   /* putting "-1" in artificaial channels */
   for (k=0;k<nwperp;k++){
      ii=nstrip+k*(nstrip+n_diff);
      jj=ii+n_diff;
      for (j=0;j<bPar->NPHUSE;++j)
      for (kk=ii;kk<jj;kk++){
         ih_inner[kk][j]=-1;
         ih_outer[kk][j]=-1;
      }
   }

   for (i=0;i< bRaw_h->nok;++i)
   if ( bRaw[i].shell==0)
      ih_inner[bRaw[i].strip+bRaw[i].cell*(nstrip+n_diff)][bRaw[i].row]=  
         bRaw[i].adc;
   else  
      ih_outer[bRaw[i].strip+bRaw[i].cell*(nstrip+n_diff)][bRaw[i].row]=
         bRaw[i].adc;

}

 

void compressOuterBarrel( DMVDPAR_ST  *Par,
                          DMVBPAR_ST *bPar,
                          float ratio,
                          int ih_comp[][6],
                          int ih_outer[][6],
                          short nzexpd,
                          short *maxzout) {

   /* Compress the array from the outer barrel to produce a new array,
   ih_comp which is now scaled down by a factor inner/outer barrel radii */

   int i,j,ilow,ihi,isiglo,isighi,ithr;
   float xinew,frac_hi,sighi;
   float ver_thrsh_mip,full_scale,smax_mip;

   ver_thrsh_mip=Par->ver_thrsh_mip;
   full_scale   =Par->full_scale;
   smax_mip     =Par->smax_mip;
  
   *maxzout=(short)((float)nzexpd*ratio)+1;
  

   if (*maxzout > nzexpd || *maxzout <=0) 
        cout << "\tcompressOuterBarrel : ERROR"
             << " output array dimension messed up="
             <<   *maxzout << endl; 

   for (j=0; j<bPar->NPHUSE; ++j)
   for (i=0; i<bPar->NZBIG; ++i)
      ih_comp[i][j]=0;
 
   /* Compress the array. This requires the shape of the outer 
   and inner barrel pulse-height arrays to be similar -- if the
   simplest possible scale-down algorithm was used, the scaled-down
   outer array would (could) have spikes in it. */
 
   for(i=0;i<nzexpd;++i) {

      xinew=(float)i*ratio;
      if(xinew <1.0 ) {
         for( j=0; j<bPar->NPHUSE; ++j){
            ih_comp[0][j]+=ih_outer[0][j];
         }
      }
      else {
         ilow=(int)xinew;
         ihi=ilow+1;

         if ( ihi > (*maxzout-1) ) {
            for ( j=0;j< bPar->NPHUSE;++j)
               ih_comp[*maxzout-1][j]
               = ih_comp[*maxzout-1][j]+ih_outer[*maxzout-1][j];
         }

         /* Make sure that channels which would be based,
         even partially, on a "fake" channel (signal=-1)
         are set to -1 in the output too. The "fake" channels
         were created in the gaps between strips and are
         needed in this algorithm to make sure that
         the position is proportional to the channel in z.  */

         else { 
            frac_hi=xinew-(float)ilow;
            for (j=0;j<bPar->NPHUSE;++j) {
               if(ih_outer[i][j] !=-1) {
                  sighi=frac_hi*(float)(ih_outer[i][j]);
                  isighi= (int)sighi;
                  isiglo=ih_outer[i][j]-isighi;
                  if ( ih_comp[ilow][j] != -1) 
                     ih_comp[ilow][j]=ih_comp[ilow][j]+isiglo;
                  if ( ih_comp[ihi][j] != -1) 
                     ih_comp[ihi][j]=ih_comp[ihi][j]+isighi;
               }
               else { 
                  ih_comp[ilow][j]=-1,
                  ih_comp[ihi][j]=-1 ;
               }
               } 
           }
       }     
   }

   /* in case of overflow , put "0" into the array  */
   ithr=(int)((ver_thrsh_mip*full_scale)/smax_mip);
   for (i=0; i< *maxzout;++i)
   for (j=0;j<bPar->NPHUSE;++j)    {
      ih_comp[i][j]=ih_comp[i][j]< 256? ih_comp[i][j] : 256;
      if ( ih_comp[i][j] >0 && ih_comp[i][j] < ithr)
         ih_comp[i][j]=0;
   }


}
 

float  calculateRms( short ioff,
                     int ih_comp[][6],
                     int ih_inner[][6],
                     int nzbig,   
                     int nzexpd,
                     int maxzout,
                     int NPH ){

/* Calculate the rms difference between IH_COMP and IH_INNER
where an offset (IOFF) is applied to the first dimension of IH_COMP.
Returns the square root of the following sum:
SUM( (IH_COMP(I+IOFF,J)-IH_INNER(I,J))**2/SIGMA**2)/NOVERLAP where 
the sum runs over I=ICHMIN,ICHMAX; J=1,NPH.
ICHIMIN and ICHMAX are calculated internally to give the maximum range
of channels which overlap in the teo arrays. NOVERLAP is the total 
number of overlapping channels. */

   int i,j,ichmin,ichmax,noverlap,idiff,isum;            
   float sum,sigma_sq;

   /* calculate min and max channels for which the two arrays overlap,
   if min is greater than the max, there is no overlap, so set the
   sum to a large value and return. */            
            
   ichmin=0> -ioff? 0 : -ioff;            
   ichmax= nzexpd < maxzout-ioff? nzexpd:  maxzout-ioff;            
            
               
   if( ichmax < ichmin)               
      return 256. ;            
              
   noverlap=0;             
   sum     =0;            
                
   for (j=0;j<NPH;++j)             
   for (i=ichmin;i< ichmax;++i){            
      if ( ih_comp[i+ioff][j] >=0 && ih_inner[i][j] >=0){
         idiff = ih_comp[i+ioff][j]- ih_inner[i][j];
         idiff *=idiff;
         isum  = ih_comp[i+ioff][j]+ ih_inner[i][j];
         isum  *=isum;

         if (isum ==0) sum=sum+1;
         else if (isum ==256*256) sum=sum+1;
         else {
            sigma_sq=( 0.25*0.25) *(float) (isum);
            sum     = sum+ (float) (idiff) /sigma_sq;
         }
         noverlap=noverlap +1;

      }
      else if (ih_comp[i+ioff][j] ==0 && ih_inner[i][j]==0){
         sum=sum+1;
         noverlap=noverlap +1;
         
      } 
      else {
          /* cout << "\tNOT used" << endl ; */
      } 
   }

   if (noverlap >0) 
      return (float)sqrt((double)sum/(double)(noverlap));
   else return 256.;
            
}