#ifndef STNTUPLE_TStnBeamAlgs
#define STNTUPLE_TStnBeamAlgs
//------------------------------------------------------------------------------
//  Definition of the algorithms to access STNTUPLE beam position block
//  Author:    Christoph Paus (MIT)
//  Date:      Jul 06 2005
//
//  Example of how to get time dependent beamlines
//
//   in BeginRun():
//
//   // Get a pointer to the Database Manager
//   TStnDBManager* dbm = TStnDBManager::Instance();
//   // New beamline storage (this is a datamember of the class)
//   fBpS = (TStnBeamPosBlock*) dbm->GetTable("SvxBeamPosBlock");
//
//   in Event(int ientry):
//
//   // z0 is the average of the raw track z0 (trk->Z0Raw())
//   TVector3 p =
//     TStnBeamAlgs::Position(fBpS,GetHeaderBlock()->EventNumber(),z0);
//   TMatrix  c3(3) =
//     TStnBeamAlgs::Cov3    (fBpS,GetHeaderBlock()->EventNumber(),z0);
//
//------------------------------------------------------------------------------
#include "TVector3.h"
#include "TMatrix.h"
#include "Stntuple/data/TStnBeamPosBlock.hh"
namespace TStnBeamAlgs {
  //----------------------------------------------------------------------------
  // C o n s t a n t s
  //----------------------------------------------------------------------------
  // Average sizes provided from Stan Lai's blessed results,
  // transmitted in email 5/7/03 -- (see also VertexObjects/src/Beamline.cc)
  const double       aEmittXFit = 1.26E-7;
  const double       aEmittYFit = 1.24E-7;
  const double       aBstarXFit = 38.6;
  const double       aBstarYFit = 38.0;
  const double       aZzeroXFit = 14.2;
  const double       aZzeroYFit = -9.2;
  // Averages in z
  const double       aZWidthX   = 0.00257;
  const double       aZWidthY   = 0.00258;
  // Derive useful quantities
  const double       corrAtZeroX =
                     aEmittXFit*(aBstarXFit+aZzeroXFit*aZzeroXFit/aBstarXFit);
  const double       corrAtZeroY =
                     aEmittYFit*(aBstarYFit+aZzeroYFit*aZzeroYFit/aBstarYFit);
  // For the beam line intervals
  const float        binWidth = 0.5e6; // bins of 1/2 million triggers
  //----------------------------------------------------------------------------
  // F u n c t i o n s
  //----------------------------------------------------------------------------
  // Vertex position and covariance matrix
  static TVector3    Position   (TStnBeamPosBlock* b, int iEvt, float z);
  static TMatrix     Cov3       (TStnBeamPosBlock* b, int iEvt, float z);
  // Offset and Slope (helpers for position)
  static TVector3    Offset     (TStnBeamPosBlock* b, int iEvt);
  static TVector3    Slope      (TStnBeamPosBlock* b, int iEvt);
  // Helpers
  static double      Width      (double z,double emitt,double bStar,double z0);
  static void        Indices    (int iEvt, int nBins,
				 int &iMatch, int &iNextMatch);
  static double      Interpolate(int iEvt,
				 int iMatch,   int    iNextMatch,
				 double match, double nextMatch);
}

//------------------------------------------------------------------------------
// I m p l e m e n t a t i o n  follows as inlines
//------------------------------------------------------------------------------
inline
TVector3 TStnBeamAlgs::Position(TStnBeamPosBlock *b, int iEvt, float z) 
{
  // Get offset and slope from the beamline block
  TVector3 offset = Offset(b,iEvt);
  TVector3 slope  = Slope (b,iEvt);
  // Return the primary vertex point
  return TVector3(offset.X()+z*slope.X(),offset.Y()+z*slope.Y(),z);
}

inline
TMatrix TStnBeamAlgs::Cov3(TStnBeamPosBlock *b, int iEvt, float z) 
{
  //----------------------------------------------------------------------------
  // We do not use the width stored in the database per run but rather
  // an average. On the other hand we do not average over the z positions
  // ------------------- This is the present default in Beamline/SvxBeam/CotBeam
  //----------------------------------------------------------------------------
  // Get indices into beamline block and create pointer to right beamline
  int   iMatch,iNextMatch;
  float cov3[3][3];
  Indices(iEvt,b->NBeamPos()-1,iMatch,iNextMatch);
  TStnBeamPos *bp = b->BeamPos(iMatch+1);
  if (! bp)                                                     // Must be there
    bp = b->BeamPos(0);
  if (bp) {
    // Create an empty covariance matrix
    // First the statistical uncertainty for the whole run
    cov3[0][0] = bp->Cov00() + z* bp->Cov02()              + z*z*bp->Cov22();
    cov3[0][1] = bp->Cov01() + z*(bp->Cov12()+bp->Cov03()) + z*z*bp->Cov23();
    cov3[1][1] = bp->Cov11() + z* bp->Cov13()              + z*z*bp->Cov33();
    // Add beam profile depending on z
    cov3[0][0] +=
      Width(z,aEmittXFit,aBstarXFit,aZzeroXFit)*
      Width(z,aEmittXFit,aBstarXFit,aZzeroXFit);
    cov3[1][1] +=
      Width(z,aEmittYFit,aBstarYFit,aZzeroYFit)*
      Width(z,aEmittYFit,aBstarYFit,aZzeroYFit);
    
    // No z information, so use beam size
    cov3[2][2] = 35.0*35.0;
  }
  else {
    printf(" TStnBeamAlgs::Cov3 ERROR requested beamline not available?\n");
    printf("                          iMatch,iNextMatch: %d, %d\n",
	   iMatch,iNextMatch);
  }
    
  return TMatrix(0,2,0,2,&(cov3[0][0]),"F");
}

inline
TVector3 TStnBeamAlgs::Offset(TStnBeamPosBlock *b, int iEvt)
{
  // Get indices into the beamline block
  int iMatch,iNextMatch;
  Indices(iEvt,b->NBeamPos()-1,iMatch,iNextMatch);
  TStnBeamPos *bThis = b->BeamPos(iMatch+1);
  TStnBeamPos *bNext = b->BeamPos(iNextMatch+1);
  if      (bThis && bNext)
    return
      TVector3(Interpolate(iEvt,iMatch,iNextMatch,bThis->X0(),bNext->X0()),
	       Interpolate(iEvt,iMatch,iNextMatch,bThis->Y0(),bNext->Y0()),0.);
  else if (bThis)
    return TVector3(bThis->X0(),bThis->Y0(),0.);
  else {
    TStnBeamPos *bAver = b->BeamPos(0);                         // Must be there
    return TVector3(bAver->X0(),bAver->Y0(),0.);
  }
}

inline
TVector3 TStnBeamAlgs::Slope(TStnBeamPosBlock *b,int iEvt) 
{
  // Get indices into the beamline block
  int iMatch,iNextMatch;
  Indices(iEvt,b->NBeamPos()-1,iMatch,iNextMatch);
  TStnBeamPos *bThis = b->BeamPos(iMatch+1);
  TStnBeamPos *bNext = b->BeamPos(iNextMatch+1);
  if      (bThis && bNext)
    return
      TVector3(Interpolate(iEvt,iMatch,iNextMatch,bThis->DxDz(),bNext->DxDz()),
	       Interpolate(iEvt,iMatch,iNextMatch,bThis->DyDz(),bNext->DyDz()),0);
  else if (bThis)
    return TVector3(bThis->DxDz(),bThis->DyDz(),0.);
  else {
    TStnBeamPos *bAver = b->BeamPos(0);                         // Must be there
    return TVector3(bAver->DxDz(),bAver->DyDz(),0.);
  }
}

inline
double TStnBeamAlgs::Width(double z,double emitt,double bStar,double z0)
{
  return sqrt(emitt*(bStar+(z-z0)*(z-z0)/bStar));
}

inline
void TStnBeamAlgs::Indices(int iEvt, int nBins, int &iMatch, int &iNextMatch)
{
  //----------------------------------------------------------------------------
  // Input:
  //   iEvt       - event number
  //   nBins      - number of different beamlines (average NOT included)
  // Output:
  //   iMatch     - index of beamline matching this event number and
  //   iNextMatch - index of beamline which is next after the match
  //----------------------------------------------------------------------------
  // Take care of simple cases
  if (nBins == 0) {
    iMatch = iNextMatch = 0;
    return;
  }
  // Start with finding the match
  iMatch = int(float(iEvt)/binWidth);
  if      (iMatch < 0)
    iMatch = 0;
  else if (iMatch > nBins-1)
    iMatch = nBins-1;
  // Now figure out who is closest
  if      (nBins == 0)
    iNextMatch = 0;
  else if (iMatch == 0)
    iNextMatch = iMatch+1;
  else if (iMatch == nBins-1)
    iNextMatch = iMatch-1;
  else {
    if (float(iEvt)/binWidth - iMatch < 0.5)
      iNextMatch = iMatch - 1;
    else
      iNextMatch = iMatch + 1;
  }

  // Final check whether the indices are in the allowed range
  if (iMatch < 0 || iMatch > nBins) {
    printf(" TStnBeamAlgs::Indices ERROR match index out of range: %d / %d\n",
	   iMatch,nBins);
    iMatch = 0;
  }
  if (iNextMatch < 0 || iNextMatch > nBins) {
    printf(" TStnBeamAlgs::Indices ERROR next index out of range: %d / %d\n",
	   iNextMatch,nBins);
    iNextMatch = 0;
  }
  //cout << " Event: "   << iEvt
  //     << " Indices: " << iMatch << " " << iNextMatch << endl << flush; 
}

inline
double TStnBeamAlgs::Interpolate(int iEvt, int iMatch, int iNextMatch,
				 double match, double nextMatch)
{
  double scale = iEvt - (iMatch+0.5)*binWidth;
  double den   = (iNextMatch-iMatch)*binWidth;
  if (den==0) {
    printf("-- > Oops, not enough beamlines to interpolate.\n");
    return match;
  }
  return match + scale*((nextMatch-match)/den);
}
#endif