// $Header: SmearTraj.cxx $
// For V3.0 of FastMC, April 2, 1999      J. Bogart
//
// Many thanks to Rasmus Munk Larsen of Stanford for the numerically-stable
// algorithm for finding the intersection of 2 circles.


#include "TMath.h"
#include <float.h>   // for DBL_EPSILON

#include "SmearTraj.h"
#define  false  0
#define  true   1

static const Double_t SBIG = 9999999.0;
static const Double_t A_LITTLE_WAYS = 0.10;   // a millimeter.

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

ClassImp(SmearTraj)

 void SmearTraj::SetParticle(McPart* pP, Int_t partIx) {
  // Compute and cache some information about this particle

  Float_t *pIpos = pP->GetPosition();
  Float_t *pImom = pP->GetMomentum();

  totalS = 0.0;
  particleIx = partIx;
  for (int ix = 0; ix < 3; ix++) {
    iP[ix] = *pIpos++;
    iM[ix] = *pImom++;  
    newP[ix] = 0.0;
    newM[ix] = 0.0;
  }
  momMag = xyMomMag = 0.0;
  sinLambda = cosLambda = 0.0;
  rad = radQ = xCenter = yCenter = 0.0;
  fieldCnv = 0.0;

   q = (int) pP->GetCharge();

  pPart = pP;
}

 void  SmearTraj::Update() {
  // Invoke this routine when the result of the last request to propagate is 
  // is deemed OK.

  for (int i = 0; i < 3; i++) {
    iP[i] = newP[i];
    iM[i] = newM[i];
  }
  totalS += deltaS;
}
  
 void SmearTraj::makeHelixQuantities(Double_t fld) {
  // For a given field (and momementum magnitude of particle which,
  // for current implementation, is constant throughoutn its life)
  // compute and store some useful quantities.

  static const Double_t VERY_SMALL_COSLAMBDA = 0.0000010;
  fieldCnv = fld * 0.003;

  momMag = TMath::Sqrt(iM[0]*iM[0] + iM[1]*iM[1] + iM[2]*iM[2]);
  

  xyMomMag = pythagPlus(iM[0], iM[1]);
  sinLambda = iM[2] / momMag;
  cosLambda = xyMomMag / momMag;
  
  if (q == 0) {
    // propagate helix stuff is irrelevant
    xCenter = yCenter  = rad = radQ = 0;
  }
  else {
    if (cosLambda < VERY_SMALL_COSLAMBDA) { // essentially just moves in z
      xCenter = iP[0];
      yCenter = iP[1];
      radQ = 0;
    }
    else {
      Double_t fieldCnvQ = fieldCnv * q;

      radQ = xyMomMag / fieldCnvQ;
      rad = TMath::Abs(radQ);
      //      xCenter = iP[0] + radQ * iM[1] / xyMomMag;
      xCenter = iP[0] + iM[1] / fieldCnvQ;
      //      yCenter = iP[1] - radQ * iM[0] / xyMomMag;
      yCenter = iP[1] - iM[0] / fieldCnvQ;
    }
  }
}

 Double_t SmearTraj::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;
  static const Double_t VERY_SMALL = 0.00001;

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

  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 = false;  
    }
    return r2;
  }
  else if (r2 < 0.0) return r1;      
  else return (r1 > r2) ? r2 : r1;  // both positive
}

 Int_t SmearTraj::circleIntersect(Double_t rv,    // radius of cylinder
				 Double_t p1[],   // (x,y) of 1st solution
				 Double_t p2[])   // (x,y) of 2nd solution
{
  // This utility is used by SmearTraj::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 x0 = xCenter;
  Double_t y0 = yCenter;
  Double_t r2 = rad;     // radius of helix
  Double_t r1 = rv;      // radius of detector volume

  Int_t    nGood = 0;

  Double_t d;   // distance between circle centers
  Double_t h; // distance from point of intersection of the two circles 
              //   to line between centers
  Double_t l; // distance from origin to intersection of two perp. lines: 
              // the line between centers and the line connecting two 
              // intersections (solutions)
  Double_t v1, v2;  // (v1, v2) is unit vector along line connecting centers.
  Double_t mag; // scale factor used in checking for solutions


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

  if ( x0==0 && y0==0 && r1==r2)
    return -1; // The two circles are identical! 


  d = pythagPlus(x0,y0); 
  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!
    v1 = x0/d; 
    v2 = y0/d;
    p1[0] = r1*v1;
    p2[0] = r1*v2;
    nGood = 1;
  }
  else {
    // Two points of intersection! 
    l = (d + (r1*(r1/d) - r2*(r2/d)))/2;
    h = pythagMinus(r1, l);
    v1 = x0/d; 
    v2 = y0/d;
    // First point (x1,y1):
    p1[0] = l*v1 + h*v2;
    p1[1] = l*v2 - h*v1;
    // Second point (x2,y2):
    p2[0] = l*v1 - h*v2;
    p2[1] = l*v2 + h*v1;
    nGood = 2;
  }

  return nGood;
}

 Double_t SmearTraj::phaseChange(Double_t newX, Double_t newY) {
  // Return change in phase needed to get to (newX, newY) from
  // initial position

  Double_t iniPhi, newPhi, delPhi;

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

  // Might want to cache iniPhi in Update method rather than re-computing
  iniPhi = TMath::ATan2(iP[1] - yCenter, iP[0] - xCenter); 
  
  newPhi = TMath::ATan2(newY - yCenter, newX - xCenter);
  
  // Which gets subtracted from which depends on sign of particle.
  // I *think* neg. charge goes "forwards" in a positive field
  delPhi = newPhi - iniPhi;          // assume forward
  if ((q * fieldCnv) > 0) delPhi = - delPhi;    // whoops, wrong guess
  if (delPhi < 0.0) delPhi += 2.0 * TMath::Pi();  // normalize
  return delPhi;
}

SmearTraj::PropagateRet 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 SmearTraj::INTERSECT_ZPLUS;       // Caller may have to fix
    }
    else {
      *pS = sIn;
      return SmearTraj::INTERSECT_CYLIN;
    }
  }
  else if (sIn < sOut) {
    *pS = sIn;
    return SmearTraj::INTERSECT_CYLIN;
  }
  else {
    *pS = sOut;
    return SmearTraj::INTERSECT_CYLOUT;
  }
}

SmearTraj::PropagateRet SmearTraj::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.

  Double_t iMomPhi;

  if (q == 0) return INTERSECT_UNCHARGED;

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

  iMomPhi = TMath::ATan2(iM[1], iM[0]);

  Double_t  phiMom = s * cosLambda / radQ - iMomPhi;
  newP[0] = xCenter + radQ * TMath::Sin(phiMom);       // x
  newP[1] = yCenter + radQ * TMath::Cos(phiMom); // y
  newP[2] = iP[2] + s * sinLambda;
  
  // Momemtum vector

  newM[0] = momMag * cosLambda * TMath::Cos(phiMom);        // x
  newM[1] = -momMag * cosLambda * TMath::Sin(phiMom); // y
  newM[2] = iM[2];                              // z - doesn't change

  deltaS = s;
  return INTERSECT_PATHOK;
}


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

  Double_t xMom = iM[0];
  Double_t yMom = iM[1];
  Double_t zMom = iM[2];

  // Momentum doesn't change in model with no energy loss.
  newM[0] = iM[0];
  newM[1] = iM[1];
  newM[2] = iM[2];

  momMag = TMath::Sqrt(xMom*xMom + yMom*yMom + zMom*zMom);

  if (momMag == 0) { // nothing happens
    newP[0] = iP[0];
    newP[1] = iP[1];
    newP[2] = iP[2];
  }
  newP[0] = iP[0] + xMom * s / momMag;
  newP[1] = iP[1] + yMom * s / momMag;
  newP[2] = iP[2] + zMom * s / momMag;
  deltaS = s;

  return INTERSECT_PATHOK;
}
SmearTraj::PropagateRet SmearTraj::HelixToCyl(SmearVolume *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.
  Double_t      iMomPhi;
  Double_t      s;
  PropagateRet  ret, ret1;
  Double_t      zCyl = pCur->outerZ;
  Double_t      rCylIn = pCur->innerR;
  Double_t      rCylOut = pCur->outerR;

  // Perhaps only for diagnostic purposes, keep coordinates of 
  // "best" intersection point with inner and outer cylinders.
  Double_t      xInner, yInner, xOuter, yOuter;
  Double_t      sol1[2], sol2[2];

  if (q == 0) return INTERSECT_ERROR;
  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.
  ret1 = HelixPath(pCur->field, A_LITTLE_WAYS);

  if (!checkInside(pCur))   return INTERSECT_ERROR;
  else Update();

  iMomPhi = TMath::ATan2(iM[1], iM[0]);

  // Which endcap, if any, are we headed towards?  Find s for endcap hit.
  Double_t  sEndcap, sBarrelOut, sBarrelIn;
  Double_t  smallSinLambda = zCyl / SBIG;

  if (sinLambda > smallSinLambda) {
    ret = INTERSECT_ZPLUS;
    sEndcap = (zCyl - iP[2]) / sinLambda;
  }
  else if (sinLambda < -smallSinLambda) {
    ret = INTERSECT_ZMINUS;
    sEndcap = -(zCyl + iP[2]) / sinLambda;
  }  
  else {   // we're headed for barrel, but set variables to something anyway
    sEndcap = SBIG;
  }

  // Find s for outer barrel
  { 
    Int_t nSol = circleIntersect(rCylOut, sol1, sol2);

    switch (nSol) {
    case 0: 
    case 1:
      sBarrelOut = SBIG;
      break;
    case 2:   // Bunch of stuff to do here
      {
	Double_t  delPhi0, delPhi1;
	delPhi0 = phaseChange(sol1[0], sol1[1]);
	delPhi1 = phaseChange(sol2[0], sol2[1]);

	if (delPhi1 < delPhi0) delPhi0 = delPhi1;
	// Now use this to calculate path length

	sBarrelOut = delPhi0 * rad / cosLambda;
	break;
      }
    default:  // anything else is screwy
      printf(" SmearTraj::HelixToCyl: Bad # intersections (%d) with outer volumen", nSol);
      return INTERSECT_ERROR;
      break;
    }
  }

  // .. and similarly for inner barrel -- if rCylIn isn't zero
  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
      {
	Double_t  delPhi0, delPhi1;
	delPhi0 = phaseChange(sol1[0], sol1[1]);
	delPhi1 = phaseChange(sol2[0], sol2[1]);

	if (delPhi1 < delPhi0) delPhi0 = delPhi1;
	// Now use this to calculate path length

	sBarrelIn = delPhi0 * rad / cosLambda;
	break;
      }
    default:  // anything else is screwy
      printf(" SmearTraj::HelixToCyl: Bad # intersections (%d) with inner volumen", nSol);
      return INTERSECT_ERROR;
      break;
    }
  }
  else sBarrelIn = SBIG;

  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
  Double_t  phiMom = s * cosLambda / radQ - iMomPhi;
  newP[0] = xCenter + radQ * TMath::Sin(phiMom);  // x
  newP[1] = yCenter + radQ * TMath::Cos(phiMom);
  newP[2] = iP[2] + s * sinLambda;

  deltaS = s;
  *pS = s + A_LITTLE_WAYS;
  
  // Momemtum vector

  newM[0] = momMag * cosLambda * TMath::Cos(phiMom);        // x
  newM[1] = -momMag * cosLambda * TMath::Sin(phiMom); // y
  newM[2] = iM[2];                              // z - doesn't change

  return ret;
}

SmearTraj::PropagateRet SmearTraj::LineToCyl(SmearVolume *pCur, Double_t *pS)
{
  // Propagate along straight line to cylinder.

  Double_t xMom, yMom, zMom;
  Double_t ix, iy, iz;
  Double_t x, y, z;
  Double_t zCyl = pCur->outerZ;
  Double_t rCylIn = pCur->innerR;
  Double_t rCylOut = pCur->outerR;

  static const Double_t SMALL_MOM = .00001;
  PropagateRet  ret, ret1;
  Double_t delX, delY, delZ;
  Double_t sEnd, sIn, sOut, s;

  *pS = 0.0;

  // Must start out inside
  ret1 = LinePath(A_LITTLE_WAYS);
  if (!checkInside(pCur)) {
    return INTERSECT_ERROR;
  }
  else Update();

  newM[0] = xMom = iM[0]; 
  newM[1] = yMom = iM[1]; 
  newM[2] = zMom = iM[2]; 

  if (xMom*xMom + yMom*yMom + zMom*zMom < .000001) return INTERSECT_NONE;

  ix = iP[0]; iy = iP[1]; iz = iP[2];

  ret = INTERSECT_NONE;

  // First check to see if we hit an endcap
  if (TMath::Abs(zMom) > SMALL_MOM) { // worth computing

    if (zMom < -SMALL_MOM) { 
      delZ = -zCyl - iz;
      ret = INTERSECT_ZMINUS;
    }
    else {
      delZ = zCyl - iz;
      ret = INTERSECT_ZPLUS;
    }
    delX =  delZ * xMom / zMom;
    delY =  delZ * yMom / zMom;
    sEnd = TMath::Sqrt(delX*delX + delY*delY + delZ*delZ);
  }
  else sEnd = SBIG;

  if (xMom*xMom + yMom*yMom < SMALL_MOM*SMALL_MOM) {
    sIn = sOut = SBIG;
  } 
  else { // Compute barrel stuff
    Double_t t;
    Bool_t   quadStatus;
    if (rCylIn <= 0.0) {  // No inner barrel.  We have solid cylinder.
      sIn = SBIG;
    }
    //    else { // if radius isn't decreasing, can't possibly go inwards
    //if (x0 * xMom + y0 * yMom <= 0.0) sIn = SBIG;
    else { 
      // 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(xMom * xMom + yMom * yMom,
		   2.0 * (xMom * ix + yMom * iy),
		   ix * ix + iy * iy - rCylIn*rCylIn, &quadStatus);

      if ((quadStatus) && (t > 0.0)) { // OK
	delX = t * xMom;
	delY = t * yMom;
	delZ = t * zMom;
	sIn = TMath::Sqrt(delX*delX + delY*delY + delZ*delZ);
      }
      else sIn = SBIG;
    }        // Done with inner barrel
    // Do same for outer barrel
    t = QuadRoot(xMom * xMom + yMom * yMom,
		 2.0 * (xMom * iy + yMom * ix),
		 ix * ix + iy * iy - rCylOut*rCylOut, &quadStatus);

    if ((quadStatus) && (t > 0.0)) { // OK
      delX = t * xMom;
      delY = t * yMom;
      delZ = t * zMom;
      sOut = TMath::Sqrt(delX*delX + delY*delY + delZ*delZ);
    }
    else sOut = SBIG;
  }

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

  // Good status.  Fill return arguments
  Double_t  absMom = TMath::Sqrt(xMom*xMom + yMom*yMom + zMom*zMom);
  delX = s * xMom / absMom;
  delY = s * yMom / absMom;
  delZ = s * zMom / absMom;

  newP[0] = ix + delX;
  newP[1]  = iy + delY;
  newP[2] = iz + delZ;

  deltaS = s;
  *pS = s + A_LITTLE_WAYS;

  return ret;
}

 Double_t SmearTraj::pythagPlus(const Double_t x, Double_t y) {
  // Compute sqrt(x*x + y*y) in maximally stable fashion
  double t, p;

  if (TMath::Abs(x) > TMath::Abs(y)) {
    t = y/x;
    p = x * TMath::Sqrt(1.0 + t*t);
  }
  else {
    t = x/y;
    p = y*TMath::Sqrt(t*t + 1.0);
  }
  //  return p;
  return TMath::Abs(p);
}
 Double_t SmearTraj::pythagMinus(const Double_t x, Double_t y) {
  // Compute sqrt(x*x - y*y) maximally stable fashion
  double t, p;

  if (TMath::Abs(x) > TMath::Abs(y)) {
    t = y/x;
    p = x * TMath::Sqrt(1.0 - t*t);
  }
  else {
    t = x/y;
    p = y*TMath::Sqrt(t*t - 1.0);
  }
  // return p;
  return TMath::Abs(p);
}