// $Header: CalSmeared.cxx $

#include "CalSmeared.h"
#include "IntObj.h"

ClassImp(CalSmeared)

//______________________________________________________________________
//                                                                       
// CalSmeared
//                                                                
// CalSmeared is the Fast MC version of a cluster (really should be
// inheriting from Event Cluster class).  In addition to minimal cluster
// attributes it also keeps some information on the "perfect" cluster
// from which it is derived.

 CalSmeared::CalSmeared(SmearTraj& traj, Bool_t hadron, SmearFuzz* pFuzz,
		       TRandom *pRand)
{
  // Usual constructor.  Inputs are
  //    -- "perfect" particle position, after propagation
  //    -- hadron/lepton boolean
  //    -- smearing parameters
  //    -- random number generator instance

  McPart*    pPart = traj.GetMc();
  Int_t      partIx = traj.GetMcIx();
  Double_t*  pPos = traj.GetNewPos();
  Double_t*  pMom = traj.GetNewMom();

  Double_t a, b;
  Double_t sigma;
  Float_t examineEnergy;
  Int_t   absPid;

  sPerfect = traj.GetS();
  mcContribs = new TObjArray(1);
  mcContribs->Add(new IntObj(partIx));
  ePerfect = pPart->GetMomentum()[3];
  pid1 = pPart->GetType();
  nPart = 1;

  if (hadron) {
    a = pFuzz->energyHadA;
    b = pFuzz->energyHadB;
  }
  else {
    a = pFuzz->energyEmA;
    b = pFuzz->energyEmB;
  }

  // Save "perfect" position vector
  // Don't yet know how to get from cluster start to cluster max
  // realistically.  For now just set them equal.
  posClusterStart[0] = posPerfect[0] = *pPos;
  posClusterStart[1] = posPerfect[1] = *(pPos + 1);
  posClusterStart[2] = posPerfect[2] = *(pPos + 2);

  energySigma = sigma = 
    (TMath::Sqrt((a * a) / ePerfect + b * b)) * ePerfect;
  energy = pRand->Gaus(ePerfect, sigma);
  if (energy < 0.0) energy = 0.0;

  // Now for position
  // First step should be to calculate shower max position from what
  // I've got (shower start position).  This will add something in
  // the direction of the momentum vector (or do I need to propagate
  // along a helix for charged particles in a field??) but I don't
  // know how to do this yet.

  // Then smear position in plane perp. to momentum vector.
  if (hadron) {
    a = pFuzz->transHadA;
    b = pFuzz->transHadB;
  }
  else {
    a = pFuzz->transEmA;
    b = pFuzz->transEmB;
  }
  // Find sigma. Use MC truth -- not smeared -- energy
  //  sigma = sqrt((a*a)/E + (b*b))
  // and use it to throw a distance in transverse plane.  
  // Also get random phi (in same plane).  Since distance can be
  // negative, just let phi range from 0 to pi.
  { 
    Double_t transSigma = TMath::Sqrt((a*a)/ePerfect + b*b);
    Double_t transDist = pRand->Gaus(0.0, transSigma);
    Double_t phi =  TMath::Pi() * pRand->Rndm(0);
    Double_t positionOffset[3]; 
    Double_t primeVec[3]; // offsets in rotated coord.
    static Double_t sqrtHalf = TMath::Sqrt(0.5);

    // errors in phi and theta added in quad. should give total trans. err
    eThetaRms = ePhiRms = transSigma * sqrtHalf; 

    // Given offset vector in rotated system with 
    // momentum vector ==> (0, 0, mag) have offset find its coordinates
    // in original frame. 
    primeVec[0] = transDist * TMath::Cos(phi);
    primeVec[1] = transDist * TMath::Sin(phi);
    primeVec[2] = 0.0;
    Rotate(primeVec, pMom, &positionOffset[0]);

    // Add on to original position vector
    for (int ix = 0; ix < 3; ix++) {
      pos[ix] = posPerfect[ix] + positionOffset[ix];
    }
  }

  // Save polar coord. version of energy position
  Double_t polar[3];
  ToPolar(&pos[0], &polar[0]);
  eTheta = polar[0];
  ePhi = polar[1];
  eR = polar[2];
}

 void CalSmeared::spewDiag(FILE* ofile) {
  // More extensive spew for debugging

  Float_t *pOrig = ((McPart *) (mcContribs->First()))->GetMomentum();
  unsigned int mcAddr = (unsigned int) mcContribs->First();
  fprintf(ofile, 
"MC addr: %i PID: %d Orig egy: %f Orig mom: ( %f, %f, %f) Smeared egy %fn",          mcAddr, pid1, ePerfect, pOrig[0], pOrig[1], pOrig[2], energy);
  fprintf(ofile, "(%f, %f, %f) => (%f, %f, %f)n", posPerfect[0],
	  posPerfect[1], posPerfect[2], pos[0], pos[1], pos[2]);
  fprintf(ofile, "Geometric path length: %f n", sPerfect);
  fprintf(ofile,"%f %f %f %f %f %f %fn", GetEnergy(), 
	  GetEnergyPhi(), GetEnergyTheta(), GetEnergyR(),
	  GetEnergyPhiRms(), GetEnergyThetaRms(), GetEnergyRRms());
}

 void CalSmeared::spew(FILE* ofile) {
  // Typical cluster output for a fastMC cluster looks like
  //        energy energyPhi energyTheta energyR phiRms thetaRms rRms
  //        0 hits
  //        1 particle(s)
  //        xxxxxxxx
  //
  // where the quantities in the first line are floating point and
  // xxxxxxxx is index (in array of all MC particles) associated with 
  // generating MC particle.
  //
  // The set of all clusters are bracketed by the lines
  //    yyy Clusters                          [before]   and
  //    end                                   [after]
  //
  //     yyy is the number of clusters to follow.

  static Int_t zero = 0;

  //  fprintf(ofile,"Clustern");
  fprintf(ofile,"%f %f %f %f %f %f %fn", GetEnergy(), 
	  GetEnergyPhi(), GetEnergyTheta(), GetEnergyR(),
	  GetEnergyPhiRms(), GetEnergyThetaRms(), GetEnergyRRms());
  fprintf(ofile,"%i hit(s)n", zero);
  fprintf(ofile,"%i particle(s)n", nPart);
  
  {
    Int_t ix;
    for (ix = 0; ix < nPart; ix++) {
      fprintf(ofile, "%i n", 
	      (unsigned int) ((IntObj *)(mcContribs->At(ix)))->Getnum());
    }
  }
}

 void CalSmeared::ToPolar(const Double_t *pRect, Double_t *pPolar) {
  // Given rectangular coords, compute polar and store in provided 3-vector.
  // (theta, phi, r).  The following conventions are used:
  // 0 <= phi < 2 pi
  // 0 < phi < pi/2 for x, y > 0.  pi/2 < phi < pi for x < 0, y > 0, etc.
  // 0 <= theta < pi; theta < pi/2 for z > 0

  Double_t r;
  Double_t theta;
  Double_t phi;
  Double_t toBeamline;

  Double_t x = *pRect;
  Double_t y = *(pRect + 1);
  Double_t z = *(pRect + 2);

  toBeamline = x*x + y*y;
  r = TMath::Sqrt(toBeamline + z*z);
  toBeamline = TMath::Sqrt(toBeamline);

  pPolar[2] = r;

  if (r <= 0.0) {
    pPolar[0] = 0.0;
    pPolar[1] = 0.0;

    return;
  }
  theta = TMath::ATan2(toBeamline, z);
  if (theta < 0) theta += TMath::Pi();

  phi = TMath::ATan2(y, x);
  if (phi < 0.0)  phi += 2 * TMath::Pi();

  pPolar[0] = theta;
  pPolar[1] = phi;

  return;
}

 void CalSmeared::Rotate(Double_t* inVec, Double_t* angleVec, 
			Double_t* result)
// Rotate something  so that z axis is "returned"
// to normalVec.  That is, if angleVec has polar coords (theta, phi, r),
//  rotate about y-axis by theta, then about z-axis by phi.

{
  // We need to rotate by theta, phi coordinates of momentum
  Double_t polar[3];
  
  ToPolar(angleVec, polar);  // Gives us theta, phi, r for normal

  Double_t temp[3];

  RotateTheta(polar[0], inVec, &temp[0]);
  RotatePhi(polar[1], &temp[0], result);
 
  return;
}

 void CalSmeared::RotateTheta(const Double_t theta, 
			     Double_t *iVec, Double_t *rVec)
// Rotate about y-axis with angle theta

{
  Double_t sinTheta = TMath::Sin(theta);
  Double_t cosTheta = TMath::Cos(theta);

  *rVec = (*iVec) * cosTheta + *(iVec + 2) * sinTheta;
  *(rVec + 1) = *(iVec + 1);
  *(rVec + 2) = *iVec * (-sinTheta) + *(iVec + 2) * cosTheta;

  return;
}

 void CalSmeared::RotatePhi(const Double_t phi, 
			   Double_t *iVec, Double_t *rVec)
// Rotate about z-axis with angle phi

{
  Double_t sinPhi = TMath::Sin(phi);
  Double_t cosPhi = TMath::Cos(phi);

  *rVec = (*iVec) * cosPhi - *(iVec + 1) * sinPhi;
  *(rVec + 1) = *iVec * (sinPhi) + *(iVec + 1) * cosPhi;
  *(rVec + 2) = *(iVec + 2);

  return;
}