// ----------------------------------------------------------------------------
// $Id: LCDTrjPart.cxx,v 1.1 2001/05/10 19:32:08 toshi Exp $
// ----------------------------------------------------------------------------
//
// $Log: LCDTrjPart.cxx,v $
// Revision 1.1  2001/05/10 19:32:08  toshi
// C++ file name convention has been changed from .C to .cxx .
//
//
#include "LCDTrjPart.h"
#include "LCDTrack.h"
#include <iostream.h>

ClassImp(LCDTrjPart)   

//______________________________________________________________________
//                                                                       
// LCDTrjPart
//                                                                
 LCDTrjPart::LCDTrjPart() {
  Init();
}

 LCDTrjPart::LCDTrjPart(LCDGetParameters* gp)  { 
  // Constructor.   Primary job is to assemble various parameters
  // describing detector geometry and resolution.
  Init();
  SetDetectorParameters(gp);
}

 void LCDTrjPart::Init() {
  nVolumes     = 0;
  pInnerVolume = 0;
  pVolumes     = 0;
  f_dblevel    = 0;
}

 void LCDTrjPart::SetDetectorParameters(LCDGetParameters* gp)  { 
  // Constructor.   Primary job is to assemble various parameters
  // describing detector geometry and resolution.
  nVolumes     = gp->GetNVolumes();
  if (nVolumes == 0) {
    Int_t ok = gp->SetDetectorVolumes();
    if (ok) {
      nVolumes     = gp->GetNVolumes();
      pInnerVolume = gp->GetInnerVolume();
      pVolumes     = gp->GetDetectorVolumes();
    } else  { // make it clear we have no detector
      nVolumes     = 0;
      pInnerVolume = 0;
      pVolumes     = 0;
      printf(" Error in TrjPart :: Cannot define detector volumes");
    }
  }else{
    pInnerVolume = gp->GetInnerVolume();
    pVolumes     = gp->GetDetectorVolumes();
  }
  
}

 void LCDTrjPart::Swim(Int_t charge,
		      const TVector3& momentum, 
		      const TVector3& position, 
		      Int_t barend, Double_t tRZ) {  

  LCDSwimTraj* pTraj=&traj;
  traj.SetParticleParams(position, momentum, charge);
    
  LCDSwimTraj::PropagateRet status;
  LCDDetectorVolume*  pCur = (LCDDetectorVolume *) pInnerVolume;

  Double_t decrS;              // measured in cm
  
  //printf("pInnerVolume=%d pVolumes=%dn",pInnerVolume,pVolumes);
  // traj has been initialized with position, momentum, charge
  
  if (f_dblevel > 10)
    cout<<"LCDTrjPart::Swim Start!!!"<<endl;

  while(!(pCur->isInside(position.X(), position.Y(), position.Z()))) {
    if (pCur->pNextZ == 0) {

      if (f_dblevel > 10) {
	cout<<"   "
	    <<"pCur->pNextZ==0!!! @ step 0"<<" "
	    <<"Z="<<pCur->outerZ<<endl;
      }

      return;
    }
    pCur = (LCDDetectorVolume *) pCur->pNextZ;
  }

  // First swim through empty central volume
  if ((pCur->field != 0.0) && (charge != 0)) { // helix
    status = pTraj->HelixToCyl(pCur, &decrS);
  } else {  // straight line
    status = pTraj->LineToCyl(pCur, &decrS);
  }

  if (f_dblevel > 10) {
    cout<<"   "
	<<"First swim status="<<(Int_t)status<<" "
	<<"z="<<pTraj->GetNewPosPtr()->Z()<<" "
	<<endl;
  }

  if ((status == LCDSwimTraj::INTERSECT_NONE) ||
      (status == LCDSwimTraj::INTERSECT_ERROR)   ) return;
  
  // Check that we didn't exit to the air volume next to the beampipe.
  TVector3 pPos(*(pTraj->GetNewPosPtr()));
  if ((status == LCDSwimTraj::INTERSECT_ZPLUS) || 
      (status == LCDSwimTraj::INTERSECT_ZMINUS)) {
    if (pCur->pNextZ == 0) {

      if (f_dblevel > 10) {
	cout<<"   "
	    <<"pCur->pNextZ==0!!! @ step1"<<endl;
      }

      return;
    }
    LCDDetectorVolume* pNext = (LCDDetectorVolume *) pCur->pNextZ;
    if ( pPos.Perp() < pNext->outerR ) {

      if (f_dblevel > 10) {
	cout<<"   "
	    <<"pPos.Perp() < pNextVolume->outerR!!!"<<" "
	    <<"pPos.Perp="<<pPos.Perp()<<" "
	    <<"outerR="<<pNext->outerR<<" "
	    <<endl;
      }

      return;
    }
  }

  pTraj->Update();

  while (kTRUE) {

    if (f_dblevel > 10) {
      cout<<"   "
	  <<"In while loop"<<" "
	  <<"status="<<(Int_t)status<<" "
	  <<endl;
    }

    switch(status) {   // find new volume
    case LCDSwimTraj::INTERSECT_ZMINUS:
    case LCDSwimTraj::INTERSECT_ZPLUS:
      pCur = (LCDDetectorVolume *) pCur->pNextZ;
      if (!pCur) {

	if (f_dblevel > 10) {
	  cout<<"   "
	      <<"pCur==0!!! in while loop"<<" "
	      <<"status="<<(Int_t)status<<" "
	      <<endl;
	}

	break;
      }
      // In the special case in which we're coming from an endcap air
      // volume headed in a z-direction, there will typically be more 
      // than one adjacent volume.  pNextZ pointer gives us innermost.
      // Check this against our current radius.  If necessary, follow
      // pNextR pointers until we find an acceptable volume
      while ( pTraj->GetNewPosPtr()->Perp() > pCur->outerR ) {
	pCur = (LCDDetectorVolume *) pCur->pNextR;
	if (!pCur) {

	  if (f_dblevel > 10) {
	    cout<<"   "
		<<"pCur==0!!! in while loop"<<" "
		<<"status="<<(Int_t)status<<" "
		<<endl;
	  }

	  break;
	}
      }
      break;
    case LCDSwimTraj::INTERSECT_CYLIN:
      // One extra thing to check here as well.  In case we go inwards, 
      // there may be more than one component next to the original. 
      // pPrevR points
      // to the one with smallest z.  If this isn't right (based on
      // new position), follow its pNextZ pointer.
      pCur = (LCDDetectorVolume *) pCur->pPrevR;
      if (!pCur) {

	if (f_dblevel > 10) {
	  cout<<"   "
	      <<"pCur==0!!! in while loop"<<" "
	      <<"status="<<(Int_t)status<<" "
	      <<endl;
	}

	break;
      }
      while (TMath::Abs(pTraj->GetNewPosPtr()->Z()) > pCur->outerZ) {
	pCur = (LCDDetectorVolume *) pCur->pNextZ;
	if (!pCur) {

	  if (f_dblevel > 10) {
	    cout<<"   "
		<<"pCur==0!!! in while loop"<<" "
		<<"status="<<(Int_t)status<<" "
		<<endl;
	  }

	  break;
	}
      }
      break;
    case LCDSwimTraj::INTERSECT_CYLOUT:
      pCur = (LCDDetectorVolume *) pCur->pNextR;
      break;
    default:
      return;
    }
    
    if (!pCur) return;  // Went outside detector
    
    // Propagate to boundary of volume
    Bool_t helix = ((pCur->field != 0.0) && (charge != 0));
    
    Int_t targetvol=1;
    Double_t tmprz=0;
    if (barend == 0) { // barrel
      if (tRZ < pCur->outerR) {
	tmprz = pCur->outerR;
	pCur->outerR = tRZ;
	targetvol=0;
      }
    } else { // endcap
      if (TMath::Abs(tRZ) < pCur->outerZ ) {
	tmprz = pCur->outerZ;
	pCur->outerZ = TMath::Abs(tRZ);
	targetvol=0;
      }
    }
    if (helix) {//helix
      status = pTraj->HelixToCyl(pCur, &decrS);
    } else {  // straight line
      status = pTraj->LineToCyl(pCur, &decrS);
    }
    if (!targetvol) {
      if (barend==0) {
	pCur->outerR=tmprz;
      } else {
	pCur->outerZ=tmprz;
      }
    }
    if ((status == LCDSwimTraj::INTERSECT_NONE) ||
	(status == LCDSwimTraj::INTERSECT_ERROR)  ) return;
    
    if (targetvol) { // keep going
      pTraj->Update();
    } else {  // We'll finish in this volume
      if (helix) {
	status = pTraj->HelixPath(pCur->field, decrS);
      } else {
	status = pTraj->LinePath(decrS); 
      }

      if (f_dblevel > 10) {
	cout<<"   "
	    <<"Swim end"<<" "
	    <<"status"<<(Int_t)status<<" "
	    <<"xposZ="<<traj.GetNewPosPtr()->Z()<<" "
	    <<"helix="<<(Int_t)helix<<" "
	    <<"tRZ="<<tRZ<<" "
	    <<endl;
      }
      
      pTraj->Update();

      if (barend == 0 || 
	  (barend != 0 && status == LCDSwimTraj::INTERSECT_ZPLUS) ||
	  (barend != 0 && status == LCDSwimTraj::INTERSECT_ZMINUS)) {
	return;
      }

    }
  }
  return;
}

 void LCDTrjPart::Swim(LCDEvent* event,Int_t itrk,Int_t barend, Double_t tRZ) {
  LCDTrack*  trk=(LCDTrack*) (event->Tracks()->At(itrk));
  LCDMcPart* mcp=(LCDMcPart*)(event->MCparticles()->At(trk->GetParticle()));
  Swim(trk,mcp,barend,tRZ);
}

 void LCDTrjPart::Swim(LCDTrack* trk,LCDMcPart* pmc,Int_t barend, Double_t tRZ){
  TVector3 ptrk(trk->GetMomentumVector(0.0));
  TVector3 xtrk(trk->GetPositionVector(0.0));

  if (f_dblevel > 10) {
    cout<<"LCDTrjPart::Swim Original Track Swim"<<endl;
  }

  Swim(trk->GetCharge(),ptrk,xtrk,barend,tRZ);
  if (barend != 0) { //endcap cluster...
    Double_t aomega=trk->GetTrackParameter(2);//=1/rho
    Double_t tanl  =trk->GetTrackParameter(4);//=tan lambda
    Double_t cosl  =1.0/TMath::Sqrt(1.0 + tanl*tanl);
    Double_t phi   =traj.GetS()*cosl*aomega;
    if (phi > 2*TMath::Pi()) {
      TVector3 xold(*(traj.GetNewPosPtr()));

      TVector3 p_orig(pmc->Get4MomentumPtr()->Vect());
      TVector3 x_orig(*(pmc->GetPositionPtr()));
      TVector3 p_poca;
      TVector3 x_poca;
      CalcPOCA(pmc->GetCharge(),trk->GetMagneticField(),
	       p_orig,x_orig,p_poca,x_poca);

      if (f_dblevel > 10) {
	cout<<"LCDTrjPart::Swim MC track Swim"<<endl;
      }

      Swim(pmc->GetCharge(),p_poca,x_poca,barend,tRZ);
      TVector3 xtru(*(traj.GetNewPosPtr()));

      Double_t omega0  =-pmc->GetCharge()*trk->GetMagneticField()
	/333.567/p_orig.Pt();
      Double_t z00     = x_poca.Z();
      Double_t tanl0   = p_orig.Pz()/p_orig.Pt();
      Double_t tmpphi  = (tRZ - z00)/tanl0*omega0;
      Int_t    n2pi    = (Int_t)(TMath::Abs(tmpphi)/TMath::Pi()/2.0);
      Double_t l       = TMath::Abs(n2pi*2*TMath::Pi()/omega0);
      Double_t zoffset = tanl0*l;
      xtrk.SetZ(xtrk.Z()+zoffset);

      if (f_dblevel > 10) {
	cout<<"LCDTrjPart::Swim new track Swim"<<endl;
      }

      Swim(trk->GetCharge(),ptrk,xtrk,barend,tRZ);

      TVector3 xnew(*(traj.GetNewPosPtr()));

      if (f_dblevel > 0) {
	cout<<"---"
	    <<"xyz mcp="<<xtru.X()<<","<<xtru.Y()<<","<<xtru.Z()<<" "
	    <<"tRZ="<<tRZ
	    <<endl;
	cout<<"   "
	    <<"xyz old="<<xold.X()<<","<<xold.Y()<<","<<xold.Z()<<" "
	    <<"dist="<<(xold-xtru).Perp()
	    <<endl;
	cout<<"   "
	    <<"xyz new="<<xnew.X()<<","<<xnew.Y()<<","<<xnew.Z()<<" "
	    <<"dist="<<(xnew-xtru).Perp()
	    <<endl;
      }

    }
  }
}

 void LCDTrjPart::CalcPOCA(Double_t  qMC,
			  Double_t  bfld_z,
			  const TVector3&  p_orig, 
			  const TVector3&  x_orig, 
			  TVector3& p_poca, 
			  TVector3& x_poca) {
  Double_t pT   = p_orig.Pt();
  Double_t tanL = p_orig.Z()/pT;
  Double_t arho = 333.567/bfld_z*pT;
  
  TVector3 nzv(0.0,0.0,qMC/TMath::Abs(qMC));
  TVector3 xxc = p_orig.Cross(nzv);
  xxc.SetZ(0.0);
  TVector3 nxxc = xxc.Unit();
  TVector3 xc   = x_orig + arho*nxxc;
  xc.SetZ(0.0);
  TVector3 nxc  = xc.Unit();
  
  TVector3 catMC=nzv.Cross(nxc);
  catMC.SetZ(0.0);
  TVector3 ncatMC=catMC.Unit();

  p_poca    = pT*ncatMC;
  p_poca.SetZ(p_orig.Z());
  
  Double_t dphi     = TMath::ASin(nxxc.Cross(nxc).Z());
  Double_t dl       =-dphi*arho*qMC;
  x_poca            = xc - arho*nxc;
  x_poca.SetZ(x_orig.Z() + dl*tanL);
  
}