/*
 *   ---- 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() {

 TRACKING_RADIUS = 129;
 TRACKING_LENGTH = 300;
 MAXITERATION = 20000;
 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(pow(Xvertex1,2)+pow(Yvertex1,2)) > TRACKING_RADIUS){return;}
  if(fabs(Zvertex1) > TRACKING_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(pow(px,2)+pow(py,2));

  double p  = sqrt(pow(px,2)+pow(py,2)+pow(pz,2));
  
  double m = sqrt(pow(Part->E,2) -  pow(Part->Px,2)+ pow(Part->Py,2)+ pow(Part->Pz,2));
  
  if(q!=0)
    {
      double e  = Part->E  / 0.003;
      double M  = sqrt(e*e - (px*px + py*py + pz*pz) );
      double vx = px/M;
      double vy = py/M;
      double vz = pz/M;
      
      /*double Bx = BField_[0];
	double By = BField_[1];
	double Bz = BField_[2];
      */
      
      double Bx = 0;
      double By = 0;
      double Bz = 3.8;
      
      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 xold=Xvertex1;        
      double yold=Yvertex1;        
      double zold=Zvertex1;       
      
      double dt = 1/p;
            
      double x = xold;
      double y = yold;        
      double z = zold;  
      
      double VTold = sqrt(pow(vx,2)+pow(vy,2));
      
      int k = 0;
      
      while(k < MAXITERATION)
	{
	  k++;
	  
	  //
	  vx += ax*dt;
	  vy += ay*dt;
	  vz += az*dt;
	  
	  double VTratio = VTold/sqrt(pow(vx,2)+pow(vy,2));
	  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) > (TRACKING_RADIUS*TRACKING_RADIUS) )
	    {
	      x /= (x*x+y*y)/ (TRACKING_RADIUS*TRACKING_RADIUS); 
	      y /= (x*x+y*y)/ (TRACKING_RADIUS*TRACKING_RADIUS); 
	      break;
	    }
	  if( fabs(z) > TRACKING_LENGTH)break;
	  if( fabs((pow(x,2)+pow(y,2)+pow(z,2)) - (pow(xold,2)+pow(yold,2)+pow(zold,2))) < MINSEGLENGTH )continue;
	  
	  xold = x;
	  yold = y;
	  zold = z;
	}	
      if(x!=0 && y!=0 && z!=0)
	{
	  double theta = fabs(atan(y/z));
	  double eta;
	  if(z > 0)eta = -log(tan(theta/2));
	  else eta = -(-log(tan(theta/2)));
	  double phi = atan2(y,x);
	  genMomentum.SetPtEtaPhiE(Part->PT,eta,phi,Part->E);
	}
      cout<<"genMomentum final "<<genMomentum.Pt()<<endl;
    }
}