/*
 *   ---- Delphes ----
 *     A Fast Simulator for general purpose LHC detector
 *       S. Ovyn ~~~~ severine.ovyn@uclouvain.be
 *
 *     Center for Particle Physics and Phenomenology (CP3)
 *     Universite Catholique de Louvain (UCL)
 *     Louvain-la-Neuve, Belgium
 *             */

#include "interface/BFieldProp.h"
#include "TRandom.h"

#include <iostream>
#include <sstream>
#include <fstream>
#include <iomanip>

#include<cmath>


using namespace std;


//------------------------------------------------------------------------------

TrackPropagation::TrackPropagation(string DetDatacard) {

 DET = new RESOLution();
 DET->ReadDataCard(DetDatacard);
 MAXITERATION = 10000;
 MINSEGLENGTH = 70;

}

void TrackPropagation::Propagation(const TRootGenParticle *Part,TLorentzVector &genMomentum)
{

  double q  = Charge(Part->PID);
  if(q==0)return;
  
  float Xvertex1 = Part->X;
  float Yvertex1 = Part->Y;
  float Zvertex1 = Part->Z;
  
  //out of trackibg coverage?
  if(sqrt(Xvertex1*Xvertex1+Yvertex1*Yvertex1) > DET->TRACK_radius){return;}
  if(fabs(Zvertex1) > DET->TRACK_length){return;}
  
  float Px = Part->Px;
  float Py = Part->Py;
  float Pz = Part->Pz;
    
  double px = Px / 0.003;
  double py = Py / 0.003;
  double pz = Pz / 0.003;
  double pt = sqrt(px*px+py*py);
  double p  = sqrt(px*px+py*py+pz*pz);
 
  if(q!=0){
     double M  = Part->M;
     double vx = px/M;
     double vy = py/M;
     double vz = pz/M;

     double Bx = DET->TRACK_bfield_x;
     double By = DET->TRACK_bfield_y;
     double Bz = DET->TRACK_bfield_z;

     double ax =  (q/M)*(Bz*vy - By*vz);
     double ay =  (q/M)*(Bx*vz - Bz*vx);
     double az =  (q/M)*(By*vx - Bx*vy);
     double dt = 1/p;
     if(pt<266 && vz < 0.0012)       dt = fabs(0.001/vz);
 
     double xold=Xvertex1;         double x=xold;
     double yold=Yvertex1;         double y=yold;
     double zold=Zvertex1;         double z=zold;

     double VTold = sqrt(vx*vx+vy*vy);
 
     int k = 0;
     
     double Radius=DET->TRACK_radius;
     double Length=DET->TRACK_length;

     while(k < MAXITERATION){
        k++;

        vx += ax*dt;
        vy += ay*dt;
        vz += az*dt;

        double VTratio = VTold/sqrt(vx*vx+vy*vy);
        vx *= VTratio;
        vy *= VTratio;

        ax = (q/M)*(Bz*vy - By*vz);
        ay = (q/M)*(Bx*vz - Bz*vx);
        az = (q/M)*(By*vx - Bx*vy);

        x  += vx*dt;
        y  += vy*dt;
        z  += vz*dt;

       if( (x*x+y*y) > Radius*Radius ){ x /= (x*x+y*y)/(Radius*Radius); y /= (x*x+y*y)/(Radius*Radius); break;}
       if( fabs(z)>Length)break;

       xold = x;
       yold = y;
       zold = z;
     }
 
      if(x!=0 && y!=0 && z!=0)
	{
	  double eta;
          float Theta = atan2(sqrt(x*x+y*y),z);
          eta  = -log(tan(Theta/2));
	  double phi = atan2(y,x);
	  genMomentum.SetPtEtaPhiE(Part->PT,eta,phi,Part->E);
	}
    }
}