//--------------------------------------------------------------------------
// Stntuple/src/TSimpleExtrapolator.cc
// Dec 15 2000 P.Murat: simpliest extrapolator possible
//
//------------------------------------------------------------------------
					// wait for ROOT implementaion 
					// of TLine3D and THelix
#include "TVector3.h"

#include "THelix3D.hh"
#include "TLine3D.hh"
#include "TG3Plane.hh"

#include "TTrajectoryPoint.hh"
// #include "TG3Tube.hh"

#include "TSimpleExtrapolator.hh"

                                        // will migrate to TStntuple soon

#include "CalorGeometry/CalParameters.hh"

ClassImp(TSimpleExtrapolator)

//_____________________________________________________________________________
//  void TSimpleExtrapolator::gustep() {
//  }

//_____________________________________________________________________________
TSimpleExtrapolator::TSimpleExtrapolator() {
  fMinPtCes = 0.8;
  fMinPtCmu = 1.2;
  fMinPtCal = 0.2;
  fMaxEta   = 1.3;
  fPoint    = new TTrajectoryPoint();
  fLine     = new TLine3D();
  fHelix    = new THelix3D();

  fBField[0] = 0;
  fBField[1] = 0;
  fBField[2] = -14.116;
}

//_____________________________________________________________________________
TSimpleExtrapolator::~TSimpleExtrapolator() {
  delete fPoint;
  delete fLine;
  delete fHelix;
}

//_____________________________________________________________________________
int TSimpleExtrapolator::GetBField(Double_t* XYZ, Double_t* BField) {
  BField[0] = fBField[0];
  BField[1] = fBField[1];
  BField[2] = fBField[2];
  return 0;
}


//_____________________________________________________________________________
int TSimpleExtrapolator::GetBField(TVector3* XYZ, Double_t* BField) {
  BField[0] = fBField[0];
  BField[1] = fBField[1];
  BField[2] = fBField[2];
  return 0;
}


//_____________________________________________________________________________
int TSimpleExtrapolator::GetBField(TTrajectoryPoint* XYZ, Double_t* BField) {
  BField[0] = fBField[0];
  BField[1] = fBField[1];
  BField[2] = fBField[2];
  return 0;
}


//_____________________________________________________________________________
int TSimpleExtrapolator::Init(Double_t* xyz, 
			      Double_t* mom, 
			      Int_t     charge, 
			      Int_t     mode)
{
  fCharge   = charge;
					// store coordinates
  fVect[0]  = xyz[0];
  fVect[1]  = xyz[1];
  fVect[2]  = xyz[2];
					// store momentum (direction cosines
					// plus the absolute value of the 
					// momentum)
  fVect[6]  = sqrt(mom[0]*mom[0]+
		   mom[1]*mom[1]+
		   mom[2]*mom[2]);

  fVect[3]  = mom[0]/fVect[6];
  fVect[4]  = mom[1]/fVect[6];
  fVect[5]  = mom[2]/fVect[6];

  return 0;
}

//_____________________________________________________________________________
//  extrapolates track to CMU
//  return codes:
//  0: OK
// -1: ptotal   < 1.
// -2: pt       < 1.
// -3: abs(eta) > 1.2
//_____________________________________________________________________________
int TSimpleExtrapolator::SwimToCmu(int               charge,
				   int               npoints, 
				   const Double_t*   radius, 
				   TTrajectoryPoint* point) 
{
  // npoints = N+1, where N is the number of output radii
  // radius - array of (N+1) trajectory points, where the first point is
  // the initial point

//    TCdfDetector* cdf  = TCdfDetector::Instance();
//    TCmuDetector*  cmu = cdf->GetCmuDetector();

					// define radius of extrapolation
  //  double r_cmu;

//    TG3Tube* tube = cmu->Tube();
//    double r_cmu  = (tube->GetRMin()+tube->GetRMax())/2.;

  //  r_cmu               = (345.09+362.5)/2.;

  const double R_COIL = 150.;
  double eta, ptot;
					// check total momentum
					// hopefully this check has already 
					// been done, but anyway...
  ptot = point[0].GetPTotal();
  TVector3 mom = *point[0].GetDirection()*ptot;
    
  if (mom.Pt() < fMinPtCmu) return -2;

  eta = mom.Eta();

  if (fabs(eta) > fMaxEta ) return -3;
 					// get the magnetic field


  double bfield[3];

  THelix3D helix;
  TLine3D  line;

  if (radius[0] < R_COIL) {
					// we will need helix, assume uniform 
					// magnetic field

    GetBField(&point[0],bfield);
    helix.Init(&point[0], charge, bfield[2]/10.);
  }

  TTrajectoryPoint  local;
  TTrajectoryPoint* ptr;

  int line_initialized = 0;

  for (int i=1; i<npoints; i++) {
					// simple extrapolator doesn't change
					// the absolute momentum of the track
    point[i].SetPTotal(ptot);
    if (radius[i] < R_COIL) {
					// using helix all the way

      helix.GetPointAtRho(&point[i],radius[i]);
    }
    else {

      if (radius[i-1] < R_COIL) {
					// extrapolate particle using helix up
					// to coil (intermediate point), then 
					// use the straight line
	ptr = &local;
	helix.GetPointAtRho(ptr,R_COIL);
      }
      else {
					// we are already outside the coil 
	ptr = &point[i-1];
      }

      if (! line_initialized) {
	line.Init(ptr);
	line_initialized = 1;
      }
      line.GetPointAtRho(&point[i],radius[i]);
    }
  }
    
  return 0;
}

//_____________________________________________________________________________
//  extrapolates track to CES
//  return codes:
//  0: OK
// -1: ptotal   < 1.
// -2: pt       < 1.
// -3: abs(eta) > 1.2
//_____________________________________________________________________________
int TSimpleExtrapolator::SwimToCes(TTrajectoryPoint* Point , int       Charge,
				   Int_t&            Side  , Int_t&    Wedge ,
				   Double_t&         XWedge, Double_t& ZWedge)
{
//    TCdfDetector* cdf  = TCdfDetector::Instance();
//    TCmuDetector*  cmu = cdf->GetCmuDetector();

					// define radius of extrapolation

  double phi, phiw, xloc, yloc, zloc, eta, ptot, bfield[3];

  const double R_CES  = 184.15;
  const double R_COIL = 150.;
					// check total momentum
					// hopefully this check has already 
					// been done, but anyway...
  ptot = Point->GetPTotal();
  TVector3 mom = *Point->GetDirection()*ptot;
    
  if (mom.Pt() < fMinPtCes ) return -2;

  eta = mom.Eta();

  if (fabs(eta) > fMaxEta) return -3;

  THelix3D helix;
  TLine3D  line;
					// we will need helix
  GetBField(Point,bfield);
  helix.Init(Point, Charge, bfield[2]/10.);

  TTrajectoryPoint  local[2];
  TTrajectoryPoint* ptr;
//-----------------------------------------------------------------------------
// simple extrapolator doesn't change the absolute momentum of the track
//-----------------------------------------------------------------------------
  local[0].SetPTotal(ptot);
//-----------------------------------------------------------------------------
// extrapolate particle using helix up to coil (intermediate point), 
// then use the straight line
//-----------------------------------------------------------------------------
  helix.GetPointAtRho(&local[0],R_COIL);
  line.Init(&local[0]);
  line.GetPointAtRho(&local[1],R_CES);
  local[1].SetPTotal(ptot);
  ptr = &local[1];
//-----------------------------------------------------------------------------
// define side
//-----------------------------------------------------------------------------
  if (ptr->Z() < 0) Side = 0;
  else             Side = 1;
//-----------------------------------------------------------------------------
// define wedge, rotate into the local coordinate system of the wedge and 
// define the local coordinates
//-----------------------------------------------------------------------------
  phi   = TVector2::Phi_0_2pi(ptr->GetPosition()->Phi());
  Wedge = (int) (phi/(TMath::Pi()/12.));
  phiw  = TMath::Pi()/12.*(Wedge+0.5);
//-----------------------------------------------------------------------------
// define CES plane and intersect track with it
//-----------------------------------------------------------------------------
  TVector3  r0(R_CES*cos(phiw),R_CES*sin(phiw),0);
  TVector3  n0(cos(phiw),sin(phiw),0);
  TVector3  intersection;

  TG3Plane  plane(r0,n0);

  plane.FindIntersection(line, intersection);
//-----------------------------------------------------------------------------
// now we need to rotate parameters of the intersection point into the local
// coordinate system of the wedge. Check: X(local) should be equal to 0
//-----------------------------------------------------------------------------
  xloc =  intersection.X()*cos(phiw)+intersection.Y()*sin(phiw)-R_CES;
  yloc = -intersection.X()*sin(phiw)+intersection.Y()*cos(phiw);
  zloc =  intersection.Z();
//-----------------------------------------------------------------------------
// local wedge coordinates: invert X and Z for the west
//-----------------------------------------------------------------------------
  if (Side == 0) {
    XWedge =  yloc;
    ZWedge = -zloc;
  }
  else {
    XWedge = -yloc;
    ZWedge =  zloc;
  }
//-----------------------------------------------------------------------------
// make the global coords of the final point accessible (thanks Martin Erdmann)
//-----------------------------------------------------------------------------
  fVout[0] = ptr->X();
  fVout[1] = ptr->Y();
  fVout[2] = ptr->Z();
  fVout[3] = ptr->Nx();
  fVout[4] = ptr->Ny();
  fVout[5] = ptr->Nz();
  fVout[6] = ptr->GetPTotal();
  
  return 0;
}

//_____________________________________________________________________________
//  extrapolates track to CCAL or PCAL
int TSimpleExtrapolator::SwimToCal(TTrajectoryPoint* Point , int       Charge)
{

  static const double R_CES  = 184.15;  //Radius of CES
  static const double R_COIL = 150.;    //Radius of Coil
  static const double Z_PES = 185.4;    //Z of PES (between layer 0 and 1)
  static const double ZEnd_CES = 230.0; //Z of end of CES

  fPoint->SetPoint(0,0,0,0,0,0,0,0);

  double ptot = Point->GetPTotal();
  double pt = ( Point->GetPTotal() * (*(Point->GetDirection())) ).Pt();
  if (pt < fMinPtCal) return -2;

  fHelix->Init(Point, Charge, fBField[2]/10.);
  static TTrajectoryPoint  local;
  local.SetPTotal(ptot);

//-----------------------------------------------------------------------------
// extrapolate particle using helix up to coil (intermediate point), 
// then use the straight line
//-----------------------------------------------------------------------------
  fHelix->GetPointAtRho(&local,R_COIL); 
  fLine->Init(&local);
  fLine->GetPointAtRho(fPoint,R_CES);

  fPoint->SetPTotal(ptot);


  if(fPoint->Z()>ZEnd_CES) {
    // if the z position at the CES  is larger than
    // z position of PES we should only extrapolate up to 
    //    
    fHelix->GetPointAtZ(fPoint,Z_PES);
  }
  else if(fPoint->Z()<-ZEnd_CES) {
    // if the z position at the CES  is larger than
    // z position of PES we should only extrapolate up to 
    //    
    fHelix->GetPointAtZ(fPoint,-Z_PES);     
  }

  
  return 0;
}

//_____________________________________________________________________________
//  extrapolates track to CMU
//  return codes:
//  0: OK
// -1: ptotal   < 1.
// -2: pt       < 1.
// -3: abs(eta) > 1.2
//_____________________________________________________________________________
int TSimpleExtrapolator::SwimToZ(int               Charge,
				 int               NPoints, 
				 const Double_t*   Z, 
				 TTrajectoryPoint* Point) 
{
  // npoints = N+1, where N is the number of output radii
  // radius - array of (N+1) trajectory points, where the first point is
  // the initial point, Z-coordinates of the rest N will be equal to Z[i]

//    TCdfDetector* cdf  = TCdfDetector::Instance();
//    TCmuDetector*  cmu = cdf->GetCmuDetector();

					// define radius of extrapolation
  //  double r_cmu;

//    TG3Tube* tube = cmu->Tube();
//    double r_cmu  = (tube->GetRMin()+tube->GetRMax())/2.;
  
  //  r_cmu               = (345.09+362.5)/2.;

				// clearly approximate values
  const double R_COIL = 150.;
  const double Z_COT  = 160.;

  double              eta, ptot, r, bfield[3];
//-----------------------------------------------------------------------------
// check total momentum: hopefully this check has already been done, 
// just in case...
//-----------------------------------------------------------------------------
  TTrajectoryPoint  *ptr, *point, local;

  point = &Point[0];

  ptot = point->GetPTotal();
  TVector3 mom = *point->GetDirection()*ptot;
    
  if (mom.Pt() < fMinPt) return -2;
  
  eta = mom.Eta();

//-----------------------------------------------------------------------------
// consider only particles with eta greater than something
//-----------------------------------------------------------------------------
  if (fabs(eta) < fMaxEta ) return -3;
 					// get the magnetic field
  GetBField(point,bfield);

  THelix3D            helix;
  TLine3D             line;

  r = point->GetPosition()->Mag();

  if ((TMath::Abs(point->Z() < Z_COT) && (r < R_COIL))) {
//-----------------------------------------------------------------------------
// extrapolation starts in the region with magnetic field, need helix
//-----------------------------------------------------------------------------
    helix.Init(&point[0], Charge, bfield[2]/10.);
  }

  int line_initialized = 0;

  for (int i=1; i<NPoints; i++) {
					// simple extrapolator doesn't change
					// value of the track absolute momentum
    point[i].SetPTotal(ptot);
    if (TMath::Abs(Z[i]) < Z_COT) {
					// using helix all the way

      helix.GetPointAtZ(&point[i],Z[i]);
    }
    else {
      if (TMath::Abs(Z[i-1]) < Z_COT) {
//-----------------------------------------------------------------------------
// extrapolate particle using helix up intil it exits COT volume,
// then use straight line
//-----------------------------------------------------------------------------
	ptr = &local;
	if (point[0].Nz() >= 0) helix.GetPointAtZ(ptr, Z_COT);
	else                    helix.GetPointAtZ(ptr,-Z_COT);
      }
      else {
					// we are already outside the COT
	ptr = &point[i-1];
      }

      ptr->SetPTotal(ptot);

      if (! line_initialized) {
	line.Init(ptr);
	line_initialized = 1;
      }
      line.GetPointAtZ(&point[i],Z[i]);
    }
  }

  return 0;
}


//_____________________________________________________________________________
int TSimpleExtrapolator::SwimToPes(TTrajectoryPoint* Point, int        Charge, 
				   Int_t&            Side , Int_t&     Wedge ,
				   Double_t&         U    , Double_t&  V)
{
  // Z of PES taken as midway between layer 0 and 1. Assume "normal" case and
  // choose the plug (EAST/WEST) based on the particle Pz. Allow (in principal)
  // for plug misalignment in Z

  static const double Z_EAST_PES =  185.4;    
  static const double Z_WEST_PES = -185.4;    
 
  double z[2];
  
  z[0]   = Point->Z();

  if (Point->Nz() > 0) {
    z[1] =  Z_EAST_PES;
    Side = 1;
  }
  else {
    z[1] =  Z_WEST_PES;
    Side = 0;
  }

  SwimToZ(Charge,2,z,Point);
//-----------------------------------------------------------------------------
// define Wedge, U and V later...
//-----------------------------------------------------------------------------
  Wedge = -1;
  U     = 1.e6;
  V     = 1.e6;

  return 0;
}

//_____________________________________________________________________________
void TSimpleExtrapolator::GetFinalPosition(double* x) {
  // returns coordinates of the extrapolated particle 
  // in the global coordinate system

  x[0] = fVout[0];
  x[1] = fVout[1];
  x[2] = fVout[2];
}

//_____________________________________________________________________________
void TSimpleExtrapolator::GetFinalDirection(double* x) {
  // returns direction cosines of the extrapolated particle in the global
  // coordinate system

  x[0] = fVout[3];
  x[1] = fVout[4];
  x[2] = fVout[5];
}

//_____________________________________________________________________________
void TSimpleExtrapolator::GetFinalMomentum(Double_t& tot) {
  tot = fVout[6];
}

//_____________________________________________________________________________
void  TSimpleExtrapolator::GetFinalResults(Double_t* xyz, 
					   Double_t* mom, 
					   Double_t& tot) {
  // coordinates

  xyz[0]  = fVout[0];
  xyz[1]  = fVout[1];
  xyz[2]  = fVout[2];
					// momentum (direction cosines
					// plus the absolute value of the 
					// momentum)
  mom[0]  = fVout[3]*fVout[6];
  mom[1]  = fVout[4]*fVout[6];
  mom[2]  = fVout[5]*fVout[6];

  tot     = fVout[6];
}


//_____________________________________________________________________________
void  TSimpleExtrapolator::GetTrajectoryPoint(TTrajectoryPoint* Point) { 
  Point->SetPoint(fVout);
}


//_____________________________________________________________________________
void TSimpleExtrapolator::GetIEtaIPhi(Int_t Side, Int_t Wedge, Double_t ZCes, 
				      Int_t& IEta, Int_t& IPhi) 
{
  // kludge: assume that ZCes is local Z coordinate at CES (ZCes always > 0) 
  // returns IEta and IPhi of the hit tower in CDF calorimeter
  // values stored in TOWER_THETA are in degrees

  const double R_CES = 184.15;

  double cos_theta = ZCes/sqrt(ZCes*ZCes+R_CES*R_CES);
  double theta     = TMath::ACos(cos_theta)*180./TMath::Pi();

  int ieta;
  int n  = TOWER_NETA/2+1;
  for (ieta=0; ieta<n; ieta++) {
    if (theta < TOWER_THETA[ieta]) break;
  }

  IPhi = Wedge;

  if (Side == 0) IEta = ieta-1;
  else           IEta = TOWER_NETA-ieta;

}

//_____________________________________________________________________________
Double_t TSimpleExtrapolator::GetNormDeltaEta(Int_t    Side, 
 					      Int_t    Wedge, 
 					      Double_t ZCes )
{
  // returns normalized delta(eta) from the middle of the closest tower
  // keep wedge around - may want to correct for the misalignments later on

//    const double R_CES = 184.15;

//    double cos_theta = ZCes/sqrt(ZCes*ZCes+R_CES*R_CES);
//    double theta     = TMath::ACos(cos_theta)*180./TMath::Pi();

//    int ieta;
//    int n  = TOWER_NETA/2+1;
//    for (ieta=0; ieta<n; ieta++) {
//      if (theta < TOWER_THETA[ieta]) break;
//    }
  
//    if (Side == 0) {
//      ieta = ieta-1;
//      th0  = (TOWER_THETA[ieta]+TOWER_THETA[ieta+1])/2.;
//      eta0 = TMath::Tan(TMath::Log
//    }
//    else {
//      ieta = TOWER_NETA-ieta;
//    }

  return -1;
}