// ----------------------------------------------------------------------------
// $Id: LCDSwimTraj.cxx,v 1.1 2001/05/10 17:56:05 toshi Exp $
// ----------------------------------------------------------------------------
//
// $Log: LCDSwimTraj.cxx,v $
// Revision 1.1  2001/05/10 17:56:05  toshi
// Changes C++ name convention from *.C to *.cxx to allow Windows usage.
//
//
// Many thanks to Rasmus Munk Larsen of Stanford for the numerically-stable
// algorithm for finding the intersection of 2 circles.


#include "LCDSwimTraj.h"
#include <iostream.h>

#define VERY_SMALL    0.00001
#define SMALL_MOM     0.00001
#define SBIG          9999999.0
#define A_LITTLE_WAYS 0.10

//______________________________________________________________________
//
// SwimTraj
//
// This class does the work of propagating particles.

ClassImp(LCDSwimTraj)

//______________________________________________________________________
 LCDSwimTraj::LCDSwimTraj() {
}
//______________________________________________________________________

//______________________________________________________________________
LCDSwimTraj::PropagateRet LCDSwimTraj::HelixToCyl(LCDDetectorVolume *pCur, 
						  Double_t *pS) {
  // Propagate along a suitable helix until it intersects the surface of a
  // cylindrical tube with inner radius rCylIn, outer radius rCylOut,
  // and ends at z = +- zCyl.  Return code indicates whether intersection was 
  // with -z end, +z end,  inner barrel surface, outer barrel surface
  // or never happened.  Return path length as well as final position 
  // and momentum vectors.
  // Initial position must be inside the tube.

  //Must have a charge...

  //cout<<"HelixToCyl:q="<<q<<endl;

  if (q == 0) return INTERSECT_ERROR;

  PropagateRet ret = INTERSECT_NONE;   // unknown at this point
  *pS = 0.0;

  // Propagate "a little ways" and then check that we're inside volume
  // (might be on the boundary to begin with).
  // A side effect of this is to compute radius, etc.
  PropagateRet ret1 = HelixPath(pCur->field, A_LITTLE_WAYS);

  if (!CheckInside(pCur)) {
    //cout<<"HelixToCyl:checkInside error xtrj="<<xtrj.Perp()<<","
    //<<xtrj.Z()<<endl;
    return INTERSECT_ERROR;
  }
  Update();

  Double_t p = ptrj.Mag();
  Double_t pt= ptrj.Pt();

  Double_t sinLambda = p > 0 ? ptrj.Pz()/p : 0.0;
  Double_t cosLambda = p > 0 ? pt       /p : 0.0;
  
  //Check ... does the helix hit the endcap?
  Double_t sEndcap=SBIG;
  Double_t zCyl = ptrj.Pz() > 0 ? pCur->outerZ : -pCur->outerZ ;
  if (sinLambda != 0) { 
    sEndcap = TMath::Abs((zCyl - xtrj.Z())/sinLambda);
    HelixPath(sEndcap-A_LITTLE_WAYS);
    ret = xnew.Z() > 0 ? INTERSECT_ZPLUS : INTERSECT_ZMINUS ;
  }

  TVector3 sol1, sol2;
  // Find s for outer barrel
  Double_t rCylOut = pCur->outerR;
  Int_t nSol = CircleIntersect(rCylOut, sol1, sol2);

  Double_t sBarrelOut=SBIG;
  Double_t omega=m_tkpar[2];
  Double_t s0out=SBIG,s1out=SBIG;
  switch (nSol) {
  case 0: 
  case 1:
    sBarrelOut = SBIG;
    break;
  case 2:   // Bunch of stuff to do here
    s0out = PhaseChange(sol1.XYvector())/omega/cosLambda;
    s1out = PhaseChange(sol2.XYvector())/omega/cosLambda;
    // Now use this to calculate path length
    sBarrelOut = s0out < s1out ? s0out : s1out;
    break;
  default:  // anything else is screwy
    printf(" LCDSwimTraj::HelixToCyl: Bad # intersections (%d) with outer volumen", nSol);
    return INTERSECT_ERROR;
    break;
  }

  // .. and similarly for inner barrel -- if rCylIn isn't zero
  Double_t rCylIn    = pCur->innerR;
  Double_t sBarrelIn = SBIG;
  Double_t s0in=SBIG,s1in=SBIG;
  if (rCylIn > .01)   { 
    Int_t nSol = CircleIntersect(rCylIn, sol1, sol2);

    switch (nSol) {
    case 0: 
    case 1:
      sBarrelIn = SBIG;
      break;
    case 2:   // Bunch of stuff to do here
      s0in = PhaseChange(sol1.XYvector())/omega/cosLambda;
      s1in = PhaseChange(sol2.XYvector())/omega/cosLambda;
      // Now use this to calculate path length
      sBarrelIn = s0in < s1in ? s0in : s1in;
      break;
    default:  // anything else is screwy
      printf(" LCDSwimTraj::HelixToCyl: Bad # intersections (%d) with inner volumen", nSol);
      return INTERSECT_ERROR;
      break;
    }
  }

  Double_t s=0;
  ret1 = ShortestPath(sEndcap, sBarrelIn, sBarrelOut, &s);
  if (ret1 != INTERSECT_ZPLUS) ret = ret1;

  if (s == SBIG) return INTERSECT_NONE;

  // In any case need to calculate all returned values
  deltaS = s;
  *pS = s + A_LITTLE_WAYS;
  HelixPath(s);

  return ret;
}
//______________________________________________________________________

//______________________________________________________________________
LCDSwimTraj::PropagateRet LCDSwimTraj::HelixPath(Double_t fld, Double_t s) {
  // Propagate along a suitable helix for length s, starting with input
  // position and momentum vectors (by default use vectors specified in
  // previous call to SetParticle). Return final position and momentum.

  if (q == 0) return INTERSECT_UNCHARGED;

  // Invoke utility to compute some useful things.
  CalcTrackParameters(fld);

  return HelixPath(s);
}
//______________________________________________________________________

//______________________________________________________________________
LCDSwimTraj::PropagateRet LCDSwimTraj::HelixPath(Double_t s) {
  // assume helix parameter was already set...

  if (q == 0) return INTERSECT_UNCHARGED;

  Double_t d0   = m_tkpar[0];
  Double_t phi0 = m_tkpar[1];
  Double_t omega= m_tkpar[2];
  Double_t z0   = m_tkpar[3];
  Double_t tanl = m_tkpar[4];

  Double_t phi  = phi0 + omega*s/TMath::Sqrt(1.0+tanl*tanl);

  // Position vector
  xnew.SetXYZ(xtrj.X() - (1.0/omega + d0)*TMath::Sin(phi0)
	      + 1.0/omega*TMath::Sin(phi),
	      xtrj.Y() + (1.0/omega + d0)*TMath::Cos(phi0)
	      - 1.0/omega*TMath::Cos(phi),
	      xtrj.Z() + z0 + s*tanl/TMath::Sqrt(1.0+tanl*tanl));
  
  // Momemtum vector
  Double_t pt=ptrj.Pt();
  pnew.SetXYZ(pt*TMath::Cos(phi),pt*TMath::Sin(phi),ptrj.Pz());

  deltaS = s;
  return INTERSECT_PATHOK;
}
//______________________________________________________________________

//______________________________________________________________________
 void LCDSwimTraj::CalcTrackParameters(Double_t bfld_z) {
  Double_t pT = ptrj.Pt();
  if (pT < SMALL_MOM) pT=SMALL_MOM;
  m_tkpar[0]= 0.0;
  m_tkpar[1]= ptrj.Phi();
  m_tkpar[2]=-q*bfld_z/333.567/pT;
  m_tkpar[3]= 0.0;
  m_tkpar[4]= ptrj.Z()/pT;
}
//______________________________________________________________________

//______________________________________________________________________
 Int_t LCDSwimTraj::CircleIntersect(Double_t rv,    // radius of cylinder
				   TVector3& p1,   // (x,y) of 1st solution
				   TVector3& p2)   // (x,y) of 2nd solution
{
  // This utility is used by SwimTraj::HelixToCy.
  //
  // Given two circles, one centered about the origin, find their points of
  // intersection.  The return value is the number of solutions found.
  // In this application, one circle is the (xy-projection) of a volume
  // boundary, always centered about the origin.  The other the helix which
  // is typically not centered about the origin.
  // This algorithm is due to Rasmus Munk Larsen.

  // Returns the number of intersections or error code:

  //   -2: Invalid input.
  //   -1: Infinitely many intersections - the two circles are identical.
  //      0: No intersection.
  //      1: 1 point of intersection, (p1[0],p1[1]) contains the 
  //           x- and y-coordinates.
  //      2: 2 point of intersection, (p1[0],p1[1]) and (p2[0],p2[1])
  //           contains the x- and y-coordinates

  Double_t d0   = m_tkpar[0];
  Double_t phi0 = m_tkpar[1];
  Double_t omega= m_tkpar[2];
  TVector3 xc(xtrj.X() - (1.0/omega + d0)*TMath::Sin(phi0),
	      xtrj.Y() + (1.0/omega + d0)*TMath::Cos(phi0),
	      0.0);
  Double_t r2   = TMath::Abs(1.0/omega);     // radius of helix
  Double_t r1   = rv;      // radius of detector volume

  Int_t    nGood = 0;

  // Check for negative radii. 
  if (r1<=0.0 || r2<=0.0) {
    fprintf(stderr,"Negative radii passed to circle_intersect:");
    fprintf(stderr,"r1 = %g, r2=%g omega=%gn", r1, r2, omega);
    return -2;
  }
  
  // Now decide how many points of intersection there are: 

  if (xc.X()==0 && xc.Y()==0 && r1==r2)
    return -1; // The two circles are identical! 


  Double_t d=xc.Perp();   // distance between circle centers

  // scale factor used in checking for solutions
  Double_t mag = TMath::Abs(r1) + TMath::Abs(r2) + TMath::Abs(d);

  if ( d > r1 +r2 + 10 * DBL_EPSILON * mag  ||  
       d < TMath::Abs(r1-r2) - 10 * DBL_EPSILON * mag) 
    return 0; // No intersections! 

  if ( TMath::Abs(d-r1-r2) <= 10*DBL_EPSILON*mag  ||  
       TMath::Abs(d-TMath::Abs(r1-r2)) <= 10*DBL_EPSILON*mag) {
    // (Maybe!) One point of intersection!
    TVector3 nxc(xc.Unit());
    p1=r1*nxc;
    p2=r1*nxc;
    nGood = 1;
  } else {
    // Two points of intersection! 
    Double_t csfi=(d*d + r2*r2 - r1*r1)/2/r2/d;
    Double_t snfi=-q/TMath::Abs(q)*TMath::Sqrt(1.0 - csfi*csfi);
    TVector3 nco(-xc.Unit());
    TVector3 ezXnco(-nco.Y(),nco.X(),0.0);
    p1=xc + r2*csfi*nco + r2*snfi*ezXnco;
    p2=xc + r2*csfi*nco - r2*snfi*ezXnco;
    nGood = 2;
  }

  return nGood;
}
//______________________________________________________________________

//______________________________________________________________________
 Double_t LCDSwimTraj::PhaseChange(TVector2 newP) {
  // Return change in phase needed to get to (newX, newY) from
  // initial position

  // ATan2 returns a value in the range -pi to pi

  Double_t d0    = m_tkpar[0];
  Double_t phi0  = m_tkpar[1];
  Double_t omega = m_tkpar[2];

  TVector2 xc(xtrj.X() - (1.0/omega + d0)*TMath::Sin(phi0),
	      xtrj.Y() + (1.0/omega + d0)*TMath::Cos(phi0));
  TVector2 xtrjxc(xtrj.X() - xc.X(), xtrj.Y() - xc.Y());
  Double_t dphi = (newP - xc).DeltaPhi(xtrjxc);
  if        (dphi < 0 && omega > 0) {
    dphi += 2*TMath::Pi(); 
  } else if (dphi > 0 && omega < 0) {
    dphi -= 2*TMath::Pi(); 
  }
  return dphi;
}
//______________________________________________________________________

//______________________________________________________________________
LCDSwimTraj::PropagateRet LCDSwimTraj::ShortestPath(Double_t sEnd, 
						    Double_t sIn, 
						    Double_t sOut, 
						    Double_t* pS) {
  // Given values for for paths to endcap, inner barrel, outer barrel,
  // put the smallest in return path argument and return an appropriate
  // status code.  This routine can't distinguish between +z and -z
  // endcap, so caller has to fix up return code afterwards in this case.

  // Shortest path gives us the surface we hit first
  if (sEnd < sOut) {
    if (sEnd < sIn) {
      *pS = sEnd;
      return LCDSwimTraj::INTERSECT_ZPLUS;       // Caller may have to fix
    } else {
      *pS = sIn;
      return LCDSwimTraj::INTERSECT_CYLIN;
    }
  } else if (sIn < sOut) {
    *pS = sIn;
    return LCDSwimTraj::INTERSECT_CYLIN;
  } else {
    *pS = sOut;
    return LCDSwimTraj::INTERSECT_CYLOUT;
  }
}
//______________________________________________________________________

//______________________________________________________________________
LCDSwimTraj::PropagateRet LCDSwimTraj::LineToCyl(LCDDetectorVolume *pCur, 
						   Double_t *pS) {
  // Propagate along straight line to cylinder.

  *pS = 0.0;

  // Must start out inside
  PropagateRet ret1 = LinePath(A_LITTLE_WAYS);
  if (!CheckInside(pCur)) {
    return INTERSECT_ERROR;
  }

  Update();

  pnew = ptrj;

  if (pnew.Mag2() < .000001) return INTERSECT_NONE;

  PropagateRet ret = INTERSECT_NONE;

  // First check to see if we hit an endcap
  Double_t zCyl = pCur->outerZ;
  TVector3 delta_X(0.,0.,0.);
  Double_t sEnd=SBIG;
  if (TMath::Abs(pnew.Z()) > SMALL_MOM) { // worth computing
    if (pnew.Z() < -SMALL_MOM) { 
      delta_X.SetZ(-zCyl - xtrj.Z() );
      ret = INTERSECT_ZMINUS;
    } else {
      delta_X.SetZ( zCyl - xtrj.Z() );
      ret = INTERSECT_ZPLUS;
    }
    delta_X.SetX(delta_X.Z()*pnew.X()/pnew.Z());
    delta_X.SetY(delta_X.Z()*pnew.Y()/pnew.Z());
    sEnd = delta_X.Mag();
  }


  Double_t rCylIn  = pCur->innerR;
  Double_t rCylOut = pCur->outerR;
  Double_t sIn  = SBIG;
  Double_t sOut = SBIG;
  if ( pnew.Pt() > SMALL_MOM) {
    Double_t t;
    Bool_t   quadStatus;
    if (rCylIn > 0.0) {
      // Find time t such that we've reached the inner barrel, that is
      // (x0 + t*px)**2 + (y0 + t*py)**2  = rCyl**2
      // Reduces to finding the positive root, if any, of the quadratic with
      //     A = px*px + py*py, B = 2(px*x0 + py*y0)
      //     C = x0*x0 + y0*y0 - rCyl*rCyl
      t = QuadRoot(pnew.Pt()*pnew.Pt(),
		   2.0*(pnew.X()*xtrj.X() + pnew.Y()*xtrj.Y()),
		   xtrj.Perp()*xtrj.Perp() - rCylIn*rCylIn, 
		   &quadStatus);

      if ((quadStatus) && (t > 0.0)) { // OK
	delta_X = t* pnew;
	sIn = delta_X.Mag();
      }
    }        // Done with inner barrel
    // Do same for outer barrel
    t = QuadRoot(pnew.Pt()*pnew.Pt(),
		 2.0*(pnew.X()*xtrj.Y() + pnew.Y()*xtrj.X()),
		 xtrj.Perp()*xtrj.Perp() - rCylOut*rCylOut, 
		 &quadStatus);

    if ((quadStatus) && (t > 0.0)) { // OK
      delta_X = t*pnew;
      sOut = delta_X.Mag();
    }
  }

  Double_t s=0;

  ret1 = ShortestPath(sEnd, sIn, sOut, &s);
  if (s == SBIG) return INTERSECT_NONE;
 
  if (ret1 != INTERSECT_ZPLUS) ret = ret1;

  // Good status.  Fill return arguments
  delta_X = (s/pnew.Mag())* pnew;
  xnew = xtrj + delta_X;

  deltaS = s;
  *pS = s + A_LITTLE_WAYS;

  return ret;
}
//______________________________________________________________________


//______________________________________________________________________
LCDSwimTraj::PropagateRet LCDSwimTraj::LinePath(Double_t s) {
  // Propagate along a line.  Same call interface as HelixPath 
  // except for fld.  

  // Momentum doesn't change in model with no energy loss.
  pnew = ptrj;

  Double_t momMag = pnew.Mag();
  if (momMag == 0) { // nothing happens
    xnew = xtrj;
  } else {
    xnew = xtrj + (s/momMag)*pnew;
  }
  deltaS = s;

  return INTERSECT_PATHOK;
}
//______________________________________________________________________

//______________________________________________________________________
 Double_t LCDSwimTraj::QuadRoot(const Double_t a, const Double_t b, 
				const Double_t c, Bool_t* status) {
  // Utility used when swimming linearly to cylinder.  Return
  // Smallest positive root, if any.

  Double_t disc = b*b - 4 * a * c;

  if ((disc < 0.0) || ((a < VERY_SMALL) && (-a > - VERY_SMALL))) {
    *status = kFALSE;
    return 0.0;
  } else {
    *status = kTRUE;
  }

  Double_t r1, r2;

  disc = TMath::Sqrt(disc);

  r1 = (-b + disc) / (2 * a);
  r2 = (-b - disc) / (2 * a);

  if (r1 < 0.0) {
    if (r2 < 0.0) {
      *status = kFALSE;  
    }
    return r2;
  }
  else if (r2 < 0.0) return r1;      
  else return (r1 > r2) ? r2 : r1;  // both positive
}
//______________________________________________________________________

//______________________________________________________________________
 void LCDSwimTraj::SetParticleParams(const TVector3& pos0, const TVector3& mom0,
				     Int_t charge)
{
  // Compute and cache some information about this particle
  
  totalS = 0.0;
  xtrj   = pos0;
  ptrj   = mom0;
  xnew   = pos0;
  pnew   = mom0;

  q = charge;
}
//______________________________________________________________________

//______________________________________________________________________
 void  LCDSwimTraj::Update() {
  // Invoke this routine when the result of the last request to propagate is 
  // is deemed OK.
  xtrj = xnew;
  ptrj = pnew;
  totalS += deltaS;
}
//______________________________________________________________________