#include "CbmKF.h"
#include "CbmKFFieldMath.h"

#include "CbmKFMath.h"
#include "CbmKFHit.h"

#include "CbmGeoNode.h"
#include "CbmRunAna.h"

#include "CbmBaseParSet.h"
#include "CbmMvdGeoPar.h"
#include "CbmGeoStsPar.h"
#include "CbmGeoRichPar.h"
#include "CbmGeoTrdPar.h"
#include "CbmGeoMuchPar.h"
#include "CbmGeoPassivePar.h"
#include "CbmStsStation.h"

#include "CbmField.h"
#include "CbmFieldConst.h"
#include "CbmFieldMap.h"
#include "CbmFieldMapSym2.h"
#include "CbmFieldMapSym3.h"
#include "CbmFieldPar.h"


ClassImp(CbmKF)

CbmKF *CbmKF::fInstance = 0;

CbmKF::CbmKF(const char *name, Int_t iVerbose ):CbmTask(name,iVerbose){
  fMethod = 1;
  fMagneticField = 0;
  if( !fInstance ) fInstance = this;
}

CbmKF::~CbmKF(){
  fInstance = 0;
}

void CbmKF::SetParContainers()
{
  CbmRunAna* ana = CbmRunAna::Instance();
  CbmRuntimeDb* rtdb=ana->GetRuntimeDb();
  rtdb->getContainer("CbmBaseParSet");
  rtdb->getContainer("CbmGeoPassivePar");
  rtdb->getContainer("CbmGeoStsPar");
  rtdb->getContainer("CbmGeoTrdPar");
  rtdb->getContainer("CbmGeoRichPar");
  rtdb->getContainer("CbmGeoMuchPar");
  rtdb->getContainer("CbmFieldPar");
  rtdb->getContainer("CbmStsDigiPar");
}

InitStatus CbmKF::Init()
{
  return ReInit();
}

InitStatus CbmKF::ReInit()
{
  fMagneticField = 0;

  vStsMaterial.clear();
  vTrdMaterial.clear();
  vRichMaterial.clear();
  vMuchMaterial.clear();
  vMuchDetectors.clear();
  vPipe.clear();
  vTargets.clear();    
  vMaterial.clear();

  StsStationIDMap.clear();
  TrdStationIDMap.clear();
  MuchStationIDMap.clear();
  fMaterialID2IndexMap.clear();

  CbmRunAna *Run = CbmRunAna::Instance();
  CbmRuntimeDb *RunDB = Run->GetRuntimeDb();

  {
    CbmGeoStsPar* StsPar = (CbmGeoStsPar*) (RunDB->findContainer("CbmGeoStsPar"));
    CbmStsDigiPar *digiPar = (CbmStsDigiPar*)(RunDB->findContainer("CbmStsDigiPar"));
    StsDigi.Init(StsPar, digiPar);
  }

  cout<<"KALMAN FILTER : === INIT MAGNETIC FIELD ==="<<endl;

  fMagneticField = (CbmField*)Run->GetField();
  //cout << "Magnetic field done" << endl;

  if( !fMagneticField ) cout<<"No Magnetic Field Found"<<endl;

  /*
  CbmFieldPar* fFieldPar = (CbmFieldPar*) RunDB->findContainer("CbmFieldPar");
  if ( ! fFieldPar ) {
    cerr << "-E-  No field parameters available!"
	 << endl;
  }
  // Instantiate correct field type
  Int_t fType = fFieldPar->GetType();
  if      ( fType == 0 ) fMagneticField = new CbmFieldConst(fFieldPar);
  else if ( fType == 1 ) fMagneticField = new CbmFieldMap(fFieldPar);
  else if ( fType == 2 ) fMagneticField = new CbmFieldMapSym2(fFieldPar);
  else if ( fType == 3 ) fMagneticField = new CbmFieldMapSym3(fFieldPar);
  else cerr << "-W- CbmRunAna::GetField: Unknown field type " << fType
	    << endl;
  cout << "New field at " << fMagneticField << ", type " << fType << endl;
  // Initialise field
  if ( fMagneticField ) {
    fMagneticField->Init();
    fMagneticField->Print();
  }
*/

/** 					*/
  // fill vector of material
    
  //=== Mvd ===
  
  CbmMvdGeoPar* MvdPar = (CbmMvdGeoPar*)(RunDB->findContainer("CbmMvdGeoPar"));
  
  if( MvdPar ){ //=== Mvd stations ===
    
    cout<<"KALMAN FILTER : === READ MVD MATERIAL ==="<<endl;
    
    int NMvdStations;
    
    TObjArray* mvdNodes = MvdPar->GetGeoSensitiveNodes();
    if ( ! mvdNodes ) {
      cout << "-E- " << GetName() << "::GetGeometry: No MVD node array"
	   << endl;
      NMvdStations = 0;
      return kERROR;
    }
    
    NMvdStations = mvdNodes->GetEntries();
    
    for ( Int_t ist = 0 ; ist < NMvdStations ; ist++ ) {
      CbmGeoNode* mvdNode = (CbmGeoNode*)mvdNodes->At(ist);
      if ( ! mvdNode ) {
	cout << "-W- CbmKF::Init: station#" << ist
	     << " not found among sensitive nodes " << endl;
	continue;
      }
      
      CbmKFTube tube;
      
      CbmGeoTransform* transform = mvdNode->getLabTransform();
      CbmGeoVector translat = transform->getTranslation();
      CbmGeoTransform center = mvdNode->getCenterPosition();
      CbmGeoVector centerV = center.getTranslation();
      TArrayD* params = mvdNode->getParameters();
      CbmGeoMedium* material = mvdNode->getMedium();
      material->calcRadiationLength();
      
      tube.ID = 1101+ist;
      tube.F = 1.;
      tube.z  = translat.Z() + centerV.Z();
      tube.dz = 2. * params->At(2);
      tube.RadLength = material->getRadiationLength();
      tube.r = params->At(0);
      tube.R = params->At(1);
      tube.rr = tube.r * tube.r;
      tube.RR = tube.R * tube.R;
      tube.ZThickness = tube.dz;
      tube.ZReference = tube.z;
      
      vMvdMaterial.push_back(tube);
      MvdStationIDMap.insert(pair<Int_t,Int_t>(tube.ID, ist ) );
      
      //cout<<" Mvd material ( id, z, dz, r, R, RadL )= ( "
      //<< tube.ID<<", " << tube.z<<", " << tube.dz
      //<<", " << tube.r<<", " << tube.R<<", " << tube.RadLength<<" )"<<endl;
      
    }
  }


  CbmGeoStsPar* StsPar = (CbmGeoStsPar*) (RunDB->findContainer("CbmGeoStsPar"));


  if( StsPar ){ //=== Sts stations ===
    
    cout<<"KALMAN FILTER : === READ STS MATERIAL ==="<<endl;

    int NStations = StsDigi.GetNStations();

    for ( Int_t ist = 0; ist<NStations; ist++ )
      {
	CbmStsStation *st = StsDigi.GetStation(ist);
	if ( !st ) continue;

	CbmKFTube tube;

	tube.ID = 1000+st->GetStationNr();
	tube.F = 1.;
	tube.z  = st->GetZ();
	tube.dz = st->GetD();
	tube.RadLength = st->GetRadLength();
	tube.r  = st->GetRmin();
	tube.R  = st->GetRmax();
	tube.rr = tube.r * tube.r;
	tube.RR = tube.R * tube.R;
	tube.ZThickness = tube.dz;
	tube.ZReference = tube.z;

	vStsMaterial.push_back(tube);
	StsStationIDMap.insert(pair<Int_t,Int_t>(tube.ID, ist ) );
	
	//cout<<" Sts material ( id, z, dz, r, R, RadL )= ( "
	//<< tube.ID<<", " << tube.z<<", " << tube.dz
	//<<", " << tube.r<<", " << tube.R<<", " << tube.RadLength<<" )"<<endl;
	
      }
  }
  
  //=== Rich ===
  
  CbmGeoRichPar *RichPar = (CbmGeoRichPar*) (RunDB->findContainer("CbmGeoRichPar"));
 
  if( RichPar ) {

    cout<<"KALMAN FILTER : === READ RICH MATERIAL ==="<<endl;

    CbmGeoNode* rich1entrance = NULL;             // entrance window
    CbmGeoNode* rich1exit = NULL;                 // exit window
    CbmGeoNode* rich1gas = NULL;
    CbmGeoNode* rich1mgl = NULL;
    
    TObjArray *NodesActive = RichPar->GetGeoSensitiveNodes();
    TObjArray *NodesPassive = RichPar->GetGeoPassiveNodes();
    //cout<<"active:"<<endl;
    for( int i=0;i<NodesActive->GetEntries(); i++) { 

      CbmGeoNode *node = dynamic_cast<CbmGeoNode*> (NodesActive->At(i));
      if ( !node ) continue;	
      TString name = node->getName();
      //cout<<name<<endl;
      if(name == "rich1mgl#1") { // RICH mirror (currently sensitive! - will change soon!)
        rich1mgl = node;
      }
    }
    
    //cout<<"passive:"<<endl;
    for( int i=0;i<NodesPassive->GetEntries(); i++) {

      CbmGeoNode *node = dynamic_cast<CbmGeoNode*> (NodesPassive->At(i));	
      if ( !node ) continue;
      
      TString name = node->getName();
      //cout<<name<<endl;
      if(name == "rich1entrance") {                  // RICH entrance window
	rich1entrance = node;
      } else if(name == "rich1mgl#1") {              // RICH mirror (see above)
        rich1mgl = node;
      } else if(name == "rich1gas1") {               // RICH gas
	rich1gas = node;
      } else if(name == "rich1exit") {               // RICH exit window
	rich1exit = node;
      }
    }

    if(rich1entrance && rich1gas && rich1mgl && rich1exit ) {
      
      // --------------- RICH entrance ---------------------------
      
      CbmKFTube kapton1;
      ReadTube( kapton1, rich1entrance);
      
      CbmKFTube kapton2;
      ReadTube( kapton2, rich1exit);
      
      CbmKFTube mirror;
      ReadTube( mirror, rich1mgl);
      
      CbmKFTube gas1;
      ReadTube( gas1, rich1gas);
      
      CbmKFTube gas2 = gas1;

      gas1.z = 0.5*( (kapton1.z + kapton1.dz/2) + (mirror.z - mirror.dz/2) );
      gas1.dz= ( (mirror.z - mirror.dz/2) - (kapton1.z + kapton1.dz/2));

      gas2.z = 0.5*( (mirror.z + mirror.dz/2) + (kapton2.z - kapton2.dz/2) );
      gas2.dz= (kapton2.z - kapton2.dz/2) - (mirror.z + mirror.dz/2);
					      

      gas1.ZThickness = gas1.dz;
      gas1.ZReference = gas1.z;
      gas2.ZThickness = gas2.dz;
      gas2.ZReference = gas2.z;


      vRichMaterial.push_back(kapton1);
      vRichMaterial.push_back(gas1);
      vRichMaterial.push_back(mirror);
      vRichMaterial.push_back(gas2);
      vRichMaterial.push_back(kapton2);
      
      //for(vector<CbmKFTube>::iterator i=vRichMaterial.begin(); i!=vRichMaterial.end(); i++){
      //cout<<" Rich material ( id, z, dz, r, R, RadL )= ( "
      //<< i->ID <<", " << i->z<<", " << i->dz
      //<<", " << i->r<<", " << i->R<<", " << i->RadLength<<" )"<<endl;	
      //}
    }        
  }
  

  //=== Trd stations ===
    
  CbmGeoTrdPar *TrdPar = (CbmGeoTrdPar*) (RunDB->findContainer("CbmGeoTrdPar"));
 
  if( TrdPar ){

    cout<<"KALMAN FILTER : === READ TRD MATERIAL ==="<<endl;
        
    TObjArray *Nodes = TrdPar->GetGeoPassiveNodes();
    for( Int_t i=0;i<Nodes->GetEntries(); i++)
      {
	CbmGeoNode *node = dynamic_cast<CbmGeoNode*> (Nodes->At(i));
	if ( !node ) continue;
	
	TString name = node->getName();
	TString Sname = node->getShapePointer()->GetName();
	
	CbmGeoVector nodeV = node->getLabTransform()->getTranslation(); //in cm
	CbmGeoVector centerV = node->getCenterPosition().getTranslation();
	TArrayD *P = node->getParameters();
	CbmGeoMedium *material = node->getMedium();	
	Int_t uid = node->getMCid();
	CbmKFWall wall;
	wall.ID = uid;
        material->calcRadiationLength();
	wall.RadLength = material->getRadiationLength();
	wall.F = 1.015*1.1*1.0322;
	wall.ZReference = nodeV.Z()+centerV.Z();
	
	if( Sname=="PGON" ) wall.ZThickness = -2.*P->At(4);
	else continue;
	Double_t zRl = ( wall.RadLength > 1.E-10 ) ?( wall.ZThickness / wall.RadLength ):0;
	//cout<<"dz,RL,dz/RL = "<<wall.ZThickness<<", "<<wall.RadLength<<", "<<zRl<<endl;
	if( zRl<0.0001 ) continue;
	TrdStationIDMap.insert(pair<Int_t,Int_t>(uid, i));
	vTrdMaterial.push_back(wall);
	//cout<<" Trd material ( id, name, z, dz, RadL )= ( "
	//<< uid<<", "<<name<<", "<< wall.ZReference<<", " << wall.ZThickness
	//<<", " << wall.RadLength<<" )"<<endl;
      }

    Nodes = TrdPar->GetGeoSensitiveNodes();
    for( Int_t i=0;i<Nodes->GetEntries(); i++)
      {
	CbmGeoNode *node = dynamic_cast<CbmGeoNode*> (Nodes->At(i));
	if ( !node ) continue;
	
	TString name = node->getName();
	TString Sname = node->getShapePointer()->GetName();
	
	CbmGeoVector nodeV = node->getLabTransform()->getTranslation(); //in cm
	CbmGeoVector centerV = node->getCenterPosition().getTranslation();
	TArrayD *P = node->getParameters();
	CbmGeoMedium *material = node->getMedium();	
//	Int_t uid = node->getMCid();
        Int_t uid = i+1;
	CbmKFWall wall;
	wall.ID = uid;
        material->calcRadiationLength();
	wall.RadLength = material->getRadiationLength();
	wall.F = 1.015*1.1*1.0322;
	wall.ZReference = nodeV.Z()+centerV.Z();
	
	if( Sname=="PGON" ) wall.ZThickness = -2.*P->At(4);
	else continue;
	Double_t zRl = ( wall.RadLength > 1.E-10 ) ?( wall.ZThickness / wall.RadLength ):0;
	//cout<<"dz,RL,dz/RL = "<<wall.ZThickness<<", "<<wall.RadLength<<", "<<zRl<<endl;
	if( zRl<0.0001 ) continue;
	TrdStationIDMap.insert(pair<Int_t,Int_t>(uid, i));
	vTrdMaterial.push_back(wall);
	//cout<<" Trd material ( id, name, z, dz, RadL )= ( "
	//<< uid<<", "<<name<<", "<< wall.ZReference<<", " << wall.ZThickness
	//<<", " << wall.RadLength<<" )"<<endl;
      }
  }
  

  //=== MUon CHambers ===
    
  CbmGeoMuchPar *MuchPar = (CbmGeoMuchPar*) (RunDB->findContainer("CbmGeoMuchPar"));
 
  if( MuchPar ){

    cout<<"KALMAN FILTER : === READ MUCH MATERIAL ==="<<endl;
    
    TObjArray *Nodes = MuchPar->GetGeoPassiveNodes();
    for( Int_t i=0;i<Nodes->GetEntries(); i++)
      {
	CbmGeoNode *node = dynamic_cast<CbmGeoNode*> (Nodes->At(i));
	if( node && TString(node->GetName())=="much1" ) continue; 
	CbmKFTube tube;
	if( ReadTube( tube, node) ) continue;

	//cout<<" Much material ( id, z, dz, r, R, RadL )= ( "
	//<< tube.ID<<", " << tube.z<<", " << tube.dz
	//<<", " << tube.r<<", " << tube.R<<", " << tube.RadLength<<" )"<<endl;
	
	vMuchMaterial.push_back(tube);
      }

    Nodes = MuchPar->GetGeoSensitiveNodes();
    int ndet=0;
    for( Int_t i=0;i<Nodes->GetEntries(); i++)
      {
	CbmGeoNode *node = dynamic_cast<CbmGeoNode*> (Nodes->At(i));	
	CbmKFTube tube;
	if( ReadTube( tube, node) ) continue;

	vMuchDetectors.push_back(tube);
	MuchStationIDMap.insert(pair<Int_t,Int_t>(tube.ID, ndet));
	ndet++;

	//cout<<" Much detector ( id, z, dz, r, R, RadL )= ( "
	//<< tube.ID<<", " << tube.z<<", " << tube.dz
	//<<", " << tube.r<<", " << tube.R<<", " << tube.RadLength<<" )"<<endl;
	
      }
  }


  // === Passive Material ===

    
  CbmGeoPassivePar* PassivePar=(CbmGeoPassivePar*)(RunDB->findContainer("CbmGeoPassivePar"));
  
  if( PassivePar ){
    
    TObjArray *Nodes = PassivePar->GetGeoPassiveNodes();
    CbmGeoNode *node=0;
    
    cout<<"KALMAN FILTER : === READ BEAM PIPE MATERIAL ==="<<endl;

    node = dynamic_cast<CbmGeoNode*> (Nodes->FindObject("pipe1")); 

    if( node ){ 
      
      TString name = node->getName();
      TString Sname = node->getShapePointer()->GetName();
      
      CbmGeoVector nodeV(0,0,0);
      if( node->getMotherNode() ) nodeV = node->getLabTransform()->getTranslation(); //in cm
      CbmGeoVector centerV = node->getCenterPosition().getTranslation();
      TArrayD *P = node->getParameters();
      Int_t NP = node->getShapePointer()->getNumParam();
      CbmGeoMedium *material = node->getMedium();
      material->calcRadiationLength();

      Double_t z = nodeV.Z()+centerV.Z();
      
      if ( Sname=="PCON" )
	{
	  Int_t Nz = (NP-3)/3;
	  for( Int_t i=0; i<Nz-1; i++ )
	    {
	      CbmKFCone cone;
	      cone.ID = node->getMCid();
	      cone.z1 = P->At(3+i*3+0) + z;
	      cone.r1 = P->At(3+i*3+1);
	      cone.R1 = P->At(3+i*3+2);
	      cone.z2 = P->At(3+(i+1)*3+0) + z;
	      cone.r2 = P->At(3+(i+1)*3+1);
	      cone.R2 = P->At(3+(i+1)*3+2);
	      cone.RadLength =  material->getRadiationLength();
	      cone.F = 1.0322;
	      cone.ZReference  = (cone.z1+cone.z2)/2;	      
	      cone.ZThickness  = 0;
	      
	      if( i<=3) vPipe.push_back(cone);
	      
	      //cout <<" Pipe material ( id, {z1, r1, R1}, {z2, r2, R2}, RL )= ( "
	      //<< node->getMCid()<<", {" 
	      //<< cone.z1<<", "<<cone.r1<<", "<<cone.R1<<"}, {"
	      //<< cone.z2<<", "<<cone.r2<<", "<<cone.R2<<"}, "<<cone.RadLength
	      //<<" )"<<endl;
	    }
	}
      else
	{
	  cout <<" unknown pipe shape : "<<Sname<<endl;
	}
    }
    
    
    cout<<"KALMAN FILTER : === READ TARGET MATERIAL ==="<<endl;
      
    node = dynamic_cast<CbmGeoNode*> (Nodes->FindObject("targ"));
    if( node ){
      CbmKFTube tube;
      if( !ReadTube( tube, node) ){
	vTargets.push_back( tube );
	//cout<<" Target material ( id, z, dz, r, R, RadL )= ( "
	//<< tube.ID<<", " << tube.z<<", " << tube.dz
	//<<", " << tube.r<<", " << tube.R<<", " << tube.RadLength<<" )"<<endl;
      }
    }
  }
    

  {
    for( vector <CbmKFCone>::iterator i = vPipe.begin(); i!=vPipe.end(); ++i )
      {
	vMaterial.push_back( &*i );
      }
    
    for( vector <CbmKFTube>::iterator i = vTargets.begin(); i!=vTargets.end();++i )
      {
	vMaterial.push_back( &*i );
      }
    
    for( vector <CbmKFTube>::iterator i=vMvdMaterial.begin();i!=vMvdMaterial.end();++i )
      {
	vMaterial.push_back( &*i );
      }

    for( vector <CbmKFTube>::iterator i=vStsMaterial.begin();i!=vStsMaterial.end();++i )
      {
	vMaterial.push_back( &*i );
      }
         
    for( vector <CbmKFTube>::iterator i=vMuchMaterial.begin();i!=vMuchMaterial.end();++i )
      {
	vMaterial.push_back( &*i );
      }

     for( vector <CbmKFTube>::iterator i=vMuchDetectors.begin();i!=vMuchDetectors.end();++i )
      {
	vMaterial.push_back( &*i );
      }

     for( vector <CbmKFTube>::iterator i=vRichMaterial.begin();i!=vRichMaterial.end();++i )
      {
	vMaterial.push_back( &*i );
      }

    for( vector <CbmKFWall>::iterator i=vTrdMaterial.begin();i!=vTrdMaterial.end();++i )
      {
	vMaterial.push_back( &*i );
      }    
    sort( vMaterial.begin(), vMaterial.end(), CbmKFMaterial::comparePDown );
    for( unsigned i=0; i<vMaterial.size(); i++ ){
      fMaterialID2IndexMap.insert(pair<Int_t,Int_t>(vMaterial[i]->ID, i));
    }    
    
  }
  return kSUCCESS;
}


Int_t CbmKF::GetMaterialIndex( Int_t uid )
{
  map<Int_t,Int_t>::iterator i = fMaterialID2IndexMap.find(uid);
  if( i != fMaterialID2IndexMap.end()){      
    return i->second;
  }
  return -1;
}


Int_t CbmKF::ReadTube( CbmKFTube &tube, CbmGeoNode *node){

  if ( !node ) return 1;	

  TString name = node->getName();
  TString Sname = node->getShapePointer()->GetName();	
  CbmGeoVector nodeV = node->getLabTransform()->getTranslation(); //in cm
  CbmGeoVector centerV = node->getCenterPosition().getTranslation();
  TArrayD *P = node->getParameters();
  Int_t NP = node->getShapePointer()->getNumParam();
  CbmGeoMedium *material = node->getMedium();
  material->calcRadiationLength();

  tube.ID = node->getMCid();
  tube.RadLength = material->getRadiationLength();
  tube.F = 1.;
  tube.z  = nodeV.Z()+centerV.Z();

  if ( Sname=="TUBS" || Sname=="TUBE" )
    {
      tube.r  = P->At(0);
      tube.R  = P->At(1);
      tube.dz = 2.*P->At(2);
    }
  else if ( Sname=="TRAP" )
    {
      tube.r  = 0;
      tube.R  = 1000;
      tube.dz = 2.*P->At(0);
    }
  else if ( Sname=="SPHE" )
    {
      tube.r  = 0;
      tube.R  = 1000;
      tube.z += 0.5*( P->At(0) + P->At(1) ); // inner & outer radius
      tube.dz = (P->At(1) - P->At(0));
    }
  else if ( Sname=="PCON" )
    {
      Int_t Nz = (NP-3)/3;
      double Z = -100000, R = -100000, z = 100000, r = 100000;
      for( Int_t i=0; i<Nz; i++ )
	{
	  double z1 = P->At(3+i*3+0);
	  double r1 = P->At(3+i*3+1);
	  double R1 = P->At(3+i*3+2);
	  if( r1<r ) r = r1;
	  if( R1>R ) R = R1;
	  if( z1<z ) z = z1;
	  if( z1>Z ) Z = z1;		  
	}
      
      tube.r  = r;
      tube.R  = R;
      tube.z += (z+Z)/2.;
      tube.dz = (Z-z);
    }  
  else {
    cout <<" -E- unknown shape : "<<Sname<<endl;
    cout <<" -E- It does not make sense to go on" <<endl;
    cout <<" -E- Stop execution here" <<endl;
    Fatal("CbmKF::ReadTube","Unknown Shape");
  }
  tube.rr = tube.r * tube.r;
  tube.RR = tube.R * tube.R;
  tube.ZThickness = tube.dz;
  tube.ZReference = tube.z;
  return 0;
}


void CbmKF::Propagate( Double_t *T, Double_t *C, Double_t z_out, Double_t QP0 ) 
{
  if( fabs(T[5]-z_out)<1.e-5 ) return;

  if ( !fMagneticField || (300<=z_out && 300<=T[5]) )
    {
      CbmKFFieldMath::ExtrapolateLine( T, C, T, C, z_out );
      return;
    }
  Double_t zz = z_out;
  if ( z_out<300. && 300<=T[5] )  CbmKFFieldMath::ExtrapolateLine( T, C, T, C, 300. );

  if ( T[5]<300. && 300.<z_out )
    {
      zz = 300.;
    }
  Bool_t repeat = 1;
  while(repeat)
    {
      const Double_t max_step = 5.;
      Double_t zzz;
      if( fabs(T[5]-zz)>max_step ) zzz = T[5]+ ( (zz>T[5]) ?max_step :-max_step);
      else { zzz = zz; repeat = 0; }
      switch( fMethod )
	{
	case 0:
	  {
	    CbmKFFieldMath::ExtrapolateLine( T, C, T, C, zzz );
	    break;
	  }
	case 1:
	  { 
	    CbmKFFieldMath::ExtrapolateALight( T, C, T, C, zzz , QP0, fMagneticField );
	    break;
	  }
	case 2:
	  { 
	    CbmKFFieldMath::ExtrapolateRK4( T, C, T, C, zzz , QP0, fMagneticField );
	    break;
	  }
	  /*
	case 3:
	  { 
	    CbmKFFieldMath::ExtrapolateACentral( T, C, T, C, zzz , QP0, fMagneticField );
	    break;
	  }
	case 4:
	  { 
	    CbmKFFieldMath::ExtrapolateAnalytic( T, C, T, C, zzz , QP0, fMagneticField, 1 );
	    break;
	  }
	case 5:
	  { 
	    CbmKFFieldMath::ExtrapolateAnalytic( T, C, T, C, zzz , QP0, fMagneticField, 2 );
	    break;
	  }
	case 6:
	  { 
	    CbmKFFieldMath::ExtrapolateAnalytic( T, C, T, C, zzz , QP0, fMagneticField, 3 );
	    break;
	  }
	  */
	}
    }
  if ( T[5]!=z_out ) CbmKFFieldMath::ExtrapolateLine( T, C, T, C, z_out );  

}

void CbmKF::PassMaterial( CbmKFTrackInterface &track, Double_t &QP0, Int_t ifst, Int_t ilst ){
  Bool_t downstream = ( ilst > ifst );
  Int_t iend = downstream ? ilst+1 :ilst-1;
  for( Int_t i = ifst; i!=iend; downstream ?++i :--i ){
    vMaterial[i]->Pass( track, downstream, QP0 );
  }
}

void CbmKF::PassMaterialBetween( CbmKFTrackInterface &track, Double_t &QP0, Int_t ifst, Int_t ilst ){
  Bool_t downstream = ( ilst > ifst );
  Int_t istart = downstream ? ifst+1 :ifst-1;
  for( Int_t i = istart; i!=ilst; downstream ?++i :--i ){
    vMaterial[i]->Pass( track, downstream, QP0 );
  }
}

void CbmKF::PassMaterialBetween( CbmKFTrackInterface &track, Double_t &QP0, CbmKFHit *fst, CbmKFHit *lst ){
  PassMaterialBetween( track, QP0, fst->MaterialIndex, lst->MaterialIndex);
}