// $Header: SmearTraj.cxx $


#include "TMath.h"

#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 = rCenter = 0.0;
  phiCenter = 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 = TMath::Sqrt(iM[0]*iM[0] + iM[1]*iM[1]);
  sinLambda = iM[2] / momMag;
  cosLambda = xyMomMag / momMag;
  
  if (q == 0) {
    // propagate helix stuff is irrelevant
    xCenter = yCenter = rCenter = phiCenter = rad = radQ = 0;
  }
  else {
    if (cosLambda < VERY_SMALL_COSLAMBDA) { // essentially just moves in z
      xCenter = iP[0];
      yCenter = iP[1];
      rad = 0;
      radQ = 0;
      phiCenter = 0;   // hardly defined
    }
    else {
      Int_t signQ = (q > 0) ? 1 : -1;

      radQ = xyMomMag / (fieldCnv * q);
      rad = radQ * signQ;

      xCenter = iP[0] + radQ * iM[1] / xyMomMag;
      yCenter = iP[1] - radQ * iM[0] / xyMomMag;
      phiCenter = TMath::ATan2(yCenter, xCenter);
    }
    rCenter = TMath::Sqrt(xCenter*xCenter + yCenter*yCenter);
  }
}

 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* x0,   // array of solution x-coord
				 Double_t* y0)   // array of solution y-coord
{
  // 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 routine shouldn't find more than two solutions, but can't 
  // possibly find more than four, so to be safe x0, y0 should be pointers
  // to arrays of 4 doubles.
  static Double_t eps = .0000001;
  Double_t xh = xCenter;
  Double_t yh = yCenter;
  Double_t rh = rad;
  Double_t dh2;  // square of distance of center of helix from origin
  Double_t dh = rCenter;
  Double_t k;    // convenient to have for calculations
  Double_t discrHalf;
  Double_t xTry, yTry;
  Int_t    nGood = 0;

  dh2 = dh * dh;

  // First check that we have any solutions at all; i.e., see if helix
  // circle is either totally inside or totally outside volume circle
  // In particular, there is no solution if dh = 0.
  if ((dh + rh < rv) || TMath::Abs((dh - rh) > rv)) return nGood;

  // For completeness should also do something about the case where the
  // 2 circles coincide (dh = 0, rv = rh)

  // Want solutions (x0, y0) such that x0**2 + y0**2 = rv**2 and
  // (x0-xh)**2 + (y0-yh)**2 = rh**2.
  // Letting...
  k = 0.5 * (rv*rv + dh2 - rh*rh);

  // .. gives   k - x0*xh = y0*yh.  Square both sides and replace
  // y0**2 with rv**2 - x0**2 to get quadratic in x0 with 
  // coefficients a = dh2, b = (-2k*xh), c = (k**2 - yh**2 * rv**2)
  discrHalf =  TMath::Sqrt(k*k*xh*xh - dh2*(k*k - yh*yh*rv*rv));
  xTry = (k * xh + discrHalf) / dh2;

  // Check xTry with positive y 
  yTry = TMath::Sqrt(rv * rv - xTry * xTry);
  if (TMath::Abs((xTry - xh) * (xTry - xh) + 
		 (yTry - yh)  * (yTry - yh)    -rh * rh) < eps) {
    *x0 = xTry; *y0 = yTry;
    nGood++;
  }
  if (yTry > eps) {  // try its opposite as well
    if (TMath::Abs((xTry - xh) * (xTry - xh) + 
		   (yTry + yh)  * (yTry + yh)    -rh * rh) < eps) {
      *(x0 + nGood) = xTry; *(y0 + nGood) = -yTry;
      nGood++;
    }
  }

  // Similarly for other possible x (don't bother if discr very small)
  if (discrHalf < eps) return nGood;

  xTry = (k * xh - discrHalf) / dh2;

  yTry = TMath::Sqrt(rv * rv - xTry * xTry);
  if (TMath::Abs((xTry - xh) * (xTry - xh) + 
		 (yTry - yh)  * (yTry - yh)    -rh * rh) < eps) {
    *(x0 + nGood) = xTry; *(y0 + nGood) = yTry;
    nGood++;
  }
  if (yTry > eps) {  // try its opposite as well
    if (TMath::Abs((xTry - xh) * (xTry - xh) + 
		   (yTry + yh)  * (yTry + yh)    -rh * rh) < eps) {
      *(x0 + nGood) = xTry; *(y0 + nGood) = -yTry;
      nGood++;
    }
  }

  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"
  delPhi = newPhi - iniPhi;          // assume forward
  if (q > 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      xSol[4], ySol[4];

  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, xSol, ySol);

    switch (nSol) {
    case 0: 
    case 1:
      sBarrelOut = SBIG;
      break;
    case 2:   // Bunch of stuff to do here
      {
	Double_t  delPhi0, delPhi1;
	delPhi0 = phaseChange(xSol[0], ySol[0]);
	delPhi1 = phaseChange(xSol[1], ySol[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, xSol, ySol);

    switch (nSol) {
    case 0: 
    case 1:
      sBarrelIn = SBIG;
      break;
    case 2:   // Bunch of stuff to do here
      {
	Double_t  delPhi0, delPhi1;
	delPhi0 = phaseChange(xSol[0], ySol[0]);
	delPhi1 = phaseChange(xSol[1], ySol[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;
}