/*
 *=====================================================
 *
 *  CBM Level 1 Reconstruction 
 *  
 *  Authors: I.Kisel,  S.Gorbunov
 *
 *  e-mail : ikisel@kip.uni-heidelberg.de 
 *
 *=====================================================
 *
 *  Finds tracks using the Cellular Automaton algorithm
 *
 */

#include "CbmL1.h"
#include "CbmKF.h"
#include "CbmKFPixelMeasurement.h"
#include "TStopwatch.h"

bool UsePDAF = 0;

void CbmL1::CATrackFinder()
{

  CbmKF &KF = *CbmKF::Instance();
   
  double time_find = 0.0, time_fit = 0.0;
  const int FIRSTCASTATION = 0;
  TStopwatch c_time;

  //cout << endl << "===> CA Track Finder ..." << endl;
  
  vRTracks.clear();
  
  /*static*/ vector<int> vecA, vecB, *vA, *vB, *vC;//vector of indices of sts hits
  vA = &vecA; vB = &vecB; // vC - temporary to exchange the vectors
  (*vA).clear(); (*vB).clear();
  
  //cout << "Triplets ... " << endl;
  
  //Acceptable error for triplet creation
  double AccErrorY[] = {0.0250, 0.0500, 0.1000, 0.1500, 0.1500, 0.1500, 0.1500, 0.1500, 0.1500, 0.1500, 0.1500, 0.1500, 0.1500, 0.1500, 0.1500, 0.1500, 0.1500, 0.1500, 0.1500, 0.1500, 0.1500, 0.1500, 0.1500, 0.1500, 0.1500, 0.1500, 0.1500}; //cm 

  // replaced, limited the number of STS stations to 7.
  // SG, changed by JH 18.8.2006
  //double AccErrorY[6] = {0.0250, 0.0500, 0.1000, 0.1500, 0.1500, 0.1500}; //0.0400;// 0.2000;//0.0400; //0.0250; //cm 
  
  double yv = 0.0, zv = 0.0;  // primary vertex at (0,0) !!!
  
  double INF = 100.0;
  
  //for (int isec = -1; isec <= 2; isec++){ // all finder
  for (int isec = 0; isec <= 1; isec++){ // 
    //for (int isec = 0; isec <= 0; isec++){ // D0 !!!
    
      //cout << "isec " << isec << " : ";
    
    TStopwatch c_time_find;
    
    if (isec ==-1) TRACK_CHI2_CUT = 10.0;
    if (isec == 0) TRACK_CHI2_CUT = 20.0;
    if (isec == 1) TRACK_CHI2_CUT = 50.0;
    if (isec == 2) TRACK_CHI2_CUT = 10.0;

    double coeff = 1.0;
    if (isec == 1) coeff =  3.0;
    if (isec == 2) coeff =  1.0;

    double MinInvMom = 1.0;
    if (isec == 1) MinInvMom =  1.0/0.2;
    if (isec == 2) MinInvMom =  1.0/0.2;

    int nlevel[NStation];
    for (int il = 0; il < NStation; ++il){
      nlevel[il] = 0;
    }

    //use constraint as a StsHit
    double SigmaTargetX, SigmaTargetY;
    
    CbmKFPixelMeasurement target_hit;
    
    if (isec <= 1){// target
      if (isec ==-1){// target
	SigmaTargetX = 0.01;
	SigmaTargetY = 0.01;
      }
      else{
	SigmaTargetX = 0.1;
	SigmaTargetY = 0.1;
      }
      
      target_hit.x = 0.0;
      target_hit.y = 0.0;
      target_hit.z = 0.0;
      target_hit.V[0] = SigmaTargetX * SigmaTargetX;
      target_hit.V[1] = 0;
      target_hit.V[2] = SigmaTargetY * SigmaTargetY;
    }
    else{//use outer radius of the 1st station as a constraint
      SigmaTargetX = KF.vStsMaterial[1].R;
      SigmaTargetY = KF.vStsMaterial[1].R;
      
      target_hit.x = 0.0;
      target_hit.y = 0.0;
      target_hit.z = KF.vStsMaterial[1].z;
      target_hit.V[0] = SigmaTargetX * SigmaTargetX;
      target_hit.V[1] = 0;
      target_hit.V[2] = SigmaTargetY * SigmaTargetY;
    }
    
    //cout<<"Triplets found " << endl;

    //create triplets and find neighbors
    vvtrip.clear();
    
    //cout << "isec " << isec << " free hits: ";
    for (int ista = 0; ista < NStation; ista++){
      int nfree = 0;
      for (int ih = StsHitsStartIndex[ista]; ih <= StsHitsStopIndex[ista]; ih++){
	
	vStsHits[ih].first_triplet = -1;
	vStsHits[ih].last_triplet  = -1;
	if (!vStsHits[ih].used) nfree++;
      }
      //cout << " " << nfree;
    }
    //cout << endl;
    
    //for (int istal = NStation-2; istal >= 0; istal--){//  //start downstream chambers
    for (int istal = NStation-2; istal >= FIRSTCASTATION; istal--){//  //start downstream chambers

      //cout << "istal " << istal << ": " << endl;

      if ((isec == 2)&&(istal<=1)) continue;
      
      int istam = istal+1;
      //hit on the middle plane to start
      int start_mhit = StsHitsStartIndex[istam];
      
      int istar = istal+2;
      //hit on the right plane to start
      //int start_rhit = StsHitsStartIndex[istar];
      
      static vector<CbmL1Triplet> vtrip; vtrip.clear();
      
      double accerroryl = coeff*AccErrorY[istal];
      double accerrorym = coeff*AccErrorY[istam];
      
      int found = 0, not_found = 0;
      
      int ntriplets = 0;
      
      (*vA).clear();
      int vAindex = -1;
      
      //cout << "A size " << (*vA).size() << ", B size " << (*vB).size() << " istal " << istal << endl;
      

      for (int ilh = StsHitsStartIndex[istal]; ilh <= StsHitsStopIndex[istal]; ilh++){
	
	vStsHits[ilh].first_hit = -1; vStsHits[ilh].last_hit = -1;
	
	if (vStsHits[ilh].used) continue;
	/*
	cout << "hits  (" << StsHitsStopIndex[istal] - StsHitsStartIndex[istal] << ") : " << ilh 
	     <<  " N " << ilh - StsHitsStartIndex[istal] 
	     << " (" << vStsHits[ilh].x() << ", " << vStsHits[ilh].y() << ", " << vStsHits[ilh].z() << ") " << endl;
	*/
	double xl = vStsHits[ilh].x();
	double yl = vStsHits[ilh].y();
	double zl = vStsHits[ilh].z();
	
	if ((isec == 2)&&(istal == 2)){//
	  if (sqrt(xl*xl+yl*yl) > 0.5*KF.vStsMaterial[istam].R) continue;
	}
	
	CbmL1Triplet trl;
	
	trl.chi2   = 0.0;
	trl.NDF    = 0;
	
	trl.mass        = 0.1395679; // pion mass
	trl.is_electron = false;
	
	trl.T[0] = target_hit.x;
	trl.T[1] = target_hit.y;
	trl.T[2] = (xl-target_hit.x)/(zl-target_hit.z);
	trl.T[3] = (yl-target_hit.y)/(zl-target_hit.z);
	
	trl.T[5] = target_hit.z;
	
	trl.T[4] =  0.0;
	
	{for (int i = 0; i < 15; i++) trl.C[i] = 0.0;}
	trl.C[ 0] = trl.C[ 2] = trl.C[ 5] = trl.C[ 9] = trl.C[14] = INF;
	
	if (isec <= 0) trl.C[14] = 1.0; // +- 1.0 GeV
	if (isec == 1) trl.C[14] = (1.0/0.2)*(1.0/0.2);
	if (isec == 2) trl.C[14] = 1.0; // +- 1.0 GeV
	
	if ((isec <= 1)||((isec == 2)&&(istal > 1))){
	  //add the target
	  target_hit.Filter( trl );
	}
	
	vStsHits[ilh].Filter( trl, 1 );
		
	//propagate to the next station
	double z_stam = KF.vStsMaterial[istam].z;   //-0.5*KF.vStsMaterial[istam].dz; // z of detection plane
	
	if (istal == 2) KF.vPipe[2].Pass( trl, 1 );	//pipe between 2 and 3
	trl.Propagate( z_stam );

	bool first_mhit = true;
	//bool first_rhit = true;
	
	for (int imh = start_mhit; imh <= StsHitsStopIndex[istam]; imh++){
	  
	  double xm = vStsHits[imh].x();
	  double ym = vStsHits[imh].y();
	  double zm = vStsHits[imh].z();
	  
	  if (isec <= 1){//only for semi-primary tracks
	    //check straight line to the vertex in YZ plane
	    double y_est = yv + (ym-yv)*(zl-zv)/(zm-zv);
	    if ( (yl - y_est) > accerroryl) continue;
	    if ( (y_est - yl) > accerroryl) break;
	    
	    if (first_mhit){
	      start_mhit  = imh;
	      first_mhit  = false;
	    }
	  }
	  
	  if (ym < trl.T[1] - 3.0*sqrt(trl.C[2])) continue;
	  if (ym > trl.T[1] + 3.0*sqrt(trl.C[2])) break;
	  
	  if ((xm < trl.T[0] - 3.0*sqrt(trl.C[0])) || (xm > trl.T[0] + 3.0*sqrt(trl.C[0]))) continue;
	  //}
	  
	  if (vStsHits[imh].used) continue;

	  if (isec == -1){ //search for the longest tracks only !!!
	    if ((istam <= 4)&&(vStsHits[imh].level != NStation - 3 - istam)) continue;
	  }
	  
	  CbmL1Triplet trm;
	  
	  trm.chi2   = trl.chi2;
	  trm.NDF    = trl.NDF;
	  
	  trm.mass        = trl.mass;
	  trm.is_electron = trl.is_electron;
	  
	  trm.T[0] = trl.T[0];
	  trm.T[1] = trl.T[1];
	  trm.T[2] = trl.T[2];
	  trm.T[3] = trl.T[3];
	  trm.T[4] = trl.T[4];
	  trm.T[5] = trl.T[5];
	  
	  {for (int i = 0; i < 15; i++) trm.C[i] = trl.C[i];}
	  
	  //add the middle hit
	  vStsHits[imh].Filter( trm, 1 );
	  
	  //if (isec <= 1){
	  if ((isec <= 1)||((isec == 2)&&(istal > 1))){          //refit the track
	    
	    {for (int i = 0; i < 15; i++) trm.C[i] = 0.0;}
	    trm.C[ 0] = trm.C[ 2] = trm.C[ 5] = trm.C[ 9] = trm.C[14] = INF;
	    
	    trm.chi2   = 0.0;
	    trm.NDF    = 0;
	    
	    trm.T[0] = target_hit.x;
	    trm.T[1] = target_hit.y;
	    trm.T[2] = (xl-target_hit.x)/(zl-target_hit.z);
	    trm.T[3] = (yl-target_hit.y)/(zl-target_hit.z);
	    trm.T[5] = target_hit.z;
	    
	    
	    //fit the target

	    target_hit.Filter( trm );
	    
	    //fit the left hit
	    vStsHits[ilh].Filter( trm, 1 );
	    
	    if (istal == 2) KF.vPipe[2].Pass( trm, 1 ); //pipe between 2 and 3
	    
	    //fit the middle hit
	    vStsHits[imh].Filter( trm, 1 );
	    
	    if (fabs(1.0/trm.T[4]) < fabs(1.0/MinInvMom)) continue; //too slow track
	  }
	  
	  (*vA).push_back(imh);
	  vAindex++;
	  
	  if (vStsHits[ilh].first_hit == -1)
	    vStsHits[ilh].first_hit = vAindex;
	  
	  vStsHits[ilh].last_hit = vAindex + 1;
	  
	  if (istar >= NStation) continue;
	  	  
	  	  
	  double z_star = KF.vStsMaterial[istar].z;         //-0.5*KF.vStsMaterial[istar].dz; // z of detection plane

	  if (istam == 2) KF.vPipe[2].Pass( trm, 1 ); //pipe between 2 and 3
	  trm.Propagate( z_star );
	  
	  double xr_minus = trm.T[0] - 5.0*sqrt(trm.C[0]);
	  double xr_plus  = trm.T[0] + 5.0*sqrt(trm.C[0]);
	  
	  double yr_minus = trm.T[1] - 5.0*sqrt(trm.C[2]);
	  double yr_plus  = trm.T[1] + 5.0*sqrt(trm.C[2]);
	  
	  int first_hit_index = vStsHits[imh].first_hit;
	  int last_hit_index  = vStsHits[imh].last_hit;
	  
	  for (int irh_index = first_hit_index; irh_index < last_hit_index; irh_index++){
	    //for (int irh = start_rhit; irh <= StsHitsStopIndex[istar]; irh++){
	    int irh = (*vB)[irh_index];
	    
	    double xr = vStsHits[irh].x();
	    double yr = vStsHits[irh].y();
	    double zr = vStsHits[irh].z();
	    
	    if ((0)&&(isec >= 2)){//secondary tracks
	      //check straight line to the vertex in YZ plane
	      double y_est = yl + (yr-yl)*(zm-zl)/(zr-zl);
	      
	      if ( (ym - y_est) > accerrorym) continue;
	      if ( (y_est - ym) > accerrorym) break;
	      /*
		if (first_rhit){
		start_rhit  = irh;
		first_rhit  = false;
		}
	      */
	    }
	    
	    if ((1)||(isec <= 1)){// all ? semi-primary tracks
	      if ((xr < xr_minus) || (xr > xr_plus)) continue;
	      if (yr < yr_minus) continue;
	      if (yr > yr_plus ) break;
	    }
	    
	    if (vStsHits[irh].used) continue;

	    if (isec == -1){ //search for the longest tracks only !!!
	      if ((istar <= 5)&&(vStsHits[irh].level != NStation - 3 - istar)) continue;

	      int first_hit_index = vStsHits[imh].first_hit;
	      int last_hit_index  = vStsHits[imh].last_hit;
	     
	      bool ok = false;
	      for (int irh_index = first_hit_index; irh_index < last_hit_index; irh_index++){
		int irh_ok = (*vB)[irh_index];
		
		if (irh == irh_ok){
		  ok = true;
		  break;
		}
	      }
	      if (!ok) continue;
	    }
	    
	    CbmL1Triplet trr;
	    
	    trr.phitl  = &(vStsHits[ilh]);
	    trr.phitm  = &(vStsHits[imh]);
	    trr.phitr  = &(vStsHits[irh]);
	    
	    trr.chi2   = trm.chi2;
	    trr.NDF    = trm.NDF;
	    
	    trr.mass        = trm.mass;
	    trr.is_electron = trm.is_electron;
	    
	    trr.T[0] = trm.T[0];
	    trr.T[1] = trm.T[1];
	    trr.T[2] = trm.T[2];
	    trr.T[3] = trm.T[3];
	    trr.T[4] = trm.T[4];
	    trr.T[5] = trm.T[5];
	    
	    {for (int i = 0; i < 15; i++) trr.C[i] = trm.C[i];}
	   	    
	    //add the right hit

	    vStsHits[irh].Filter( trr, 1 );

	    if (isec >= 2){//refit the triplet
	      
	      {for (int i = 0; i < 15; i++) trr.C[i] = 0.0;}
	      trr.C[ 0] = trr.C[ 2] = trr.C[ 5] = trr.C[ 9] = trr.C[14] = INF;
	      
	      trr.chi2   = 0.0;
	      trr.NDF    = 0;
	      
	      trr.T[0] = xl;
	      trr.T[1] = yl;
	      trr.T[2] = (xl-target_hit.x)/(zl-target_hit.z);
	      trr.T[3] = (yl-target_hit.y)/(zl-target_hit.z);
	      trr.T[5] = zl;
	      	      
	      //fit the left hit

	      vStsHits[ilh].Filter( trr, 1 );
	      
	      if (istal == 2) KF.vPipe[2].Pass( trr, 1 ); //pipe between 2 and 3
	      
	      //fit the middle hit

	      vStsHits[imh].Filter( trr, 1 );
	      
	      if (istam == 2) KF.vPipe[2].Pass( trr, 1 ); //pipe between 2 and 3
	      
	      //fit the right hit
	      vStsHits[irh].Filter( trr, 1 );

	    }// isec>=2
	    
	    if (trr.NDF > 5){
	      trr.chi2 /= (trr.NDF-5); //normalize
	    }
	    
	    if (trr.chi2 > TRACK_CHI2_CUT) continue;// too bad 
	    //cout<<irh<<":"<<xr<<","<<yr<<","<<zr<<", chi2="<<trr.chi2<<endl;
	    //apply cuts on triplet parameters
	    //if (fabs(1.0/trr.T[4]) < 0.2) continue; // !!! cut of very slow tracks < 0.2 GeV
	    if (fabs(1.0/trr.T[4]) < fabs(1.0/MinInvMom)) continue; // !!! cut of very slow tracks < 0.2 GeV

	    // do we need all thees calculations to check that the track is slow? 
	    // can it be simpler and faster?
	    
	    // sort hits in slices for faster cache access?

	    
	    //OK, accept the triplet
	    
	    trr.good = false;
	    
	    trr.first_triplet = -1; 
	    trr.last_triplet  = -1;
	    
	    trr.level = 0;
	    if (vStsHits[ilh].level < trr.level  ) vStsHits[ilh].level = trr.level  ;
	    if (vStsHits[imh].level < trr.level-1) vStsHits[imh].level = trr.level-1;
	    if (vStsHits[irh].level < trr.level-2) vStsHits[irh].level = trr.level-2;
	    
	    if (istar != NStation-1){ //find the highest level among neighbors
	      //reverse order of vectors
	      int vecindex = (NStation-1) - istar;
	      vector<CbmL1Triplet> *pvtrip;
	      pvtrip = &vvtrip[vecindex];
	      
	      int first_triplet = vStsHits[imh].first_triplet;
	      int last_triplet  = vStsHits[imh].last_triplet;
	      
	      for (int index = first_triplet; index < last_triplet; ++index){ 
		CbmL1Triplet *ptrip; ptrip = &(*pvtrip)[index];
		
		if (trr.phitr != ptrip->phitm) continue;
		
		bool accept_triplet = true;
		
		if (fabs(trr.T[4] - ptrip->T[4])/sqrt(trr.C[14]) > 10.0) accept_triplet = false;
		// /*		
		int IDl = trr.phitl->MC_Point;
		int IDm = trr.phitm->MC_Point;
		int IDr = trr.phitr->MC_Point;
		if ((IDl >=0) && (IDm >=0) && (IDr >=0) ){
		  IDl = vMCPoints[IDl].ID;
		  IDm = vMCPoints[IDm].ID;
		  IDr = vMCPoints[IDr].ID;
		  if ((IDl == IDm) && (IDl == IDr)){ //correct triplet 
		    if (IDl == IDr){ //from the same track
		      
		      if (accept_triplet){
			found++;
		      }
		      else{
			not_found++;
		      }
		      
		    }
		  }
		}
		// */
		if (!accept_triplet) continue;
		
		trr.good = ptrip->good = true;
		
		if (trr.first_triplet == -1)
		  trr.first_triplet = index; 
		trr.last_triplet  = index;
		
		//OK, modify the level
		if (ptrip->level >= trr.level){ 
		  trr.level = ptrip->level + 1;

		  if (vStsHits[ilh].level < trr.level  ) vStsHits[ilh].level = trr.level  ;
		  if (vStsHits[imh].level < trr.level-1) vStsHits[imh].level = trr.level-1;
		  if (vStsHits[irh].level < trr.level-2) vStsHits[irh].level = trr.level-2;
		}
	      }
	    }
	    
	    if (trr.last_triplet != -1) trr.last_triplet++;
	    
	    //count statistics
	    nlevel[trr.level]++;
	    
	    vtrip.push_back(trr);
	  
	    //modify hit pointers
	    if (vStsHits[ilh].first_triplet == -1)
	      vStsHits[ilh].first_triplet = ntriplets;
	    vStsHits[ilh].last_triplet = ++ntriplets;
	    
	  }//irh
	}//imh
      }//ilh
      //cout << vtrip.size() << " ";
      vvtrip.push_back(vtrip);
      
      vC = vA; vA = vB; vB = vC;
      
      //cout << "*** stal " << istal << " : " << ntriplets << " (" << found << "/" << not_found << ")" << endl;
    }
    //cout<<endl;
    
    c_time_find.Stop();    
    time_find += double(c_time_find.CpuTime()); //ms
    
    /*
    cout<<"Triplets with levels [0..max] ";
    for (int ilevel = 0; ilevel < NStation-1; ++ilevel){
      cout<<nlevel[ilevel]<<" ";
    }
    cout<<endl;
    
    int n0 = 0;
    for (vector<vector<CbmL1Triplet> >::reverse_iterator vtriprIt = vvtrip.rbegin();
	 vtriprIt != vvtrip.rend(); ++vtriprIt){
      for (vector<CbmL1Triplet>::iterator tripIt = vtriprIt->begin();
	   tripIt != vtriprIt->end(); ++tripIt){
	//check the level
	if (tripIt->level != 0) continue;
	if (tripIt->good)
	  n0++;
      }
    }
    cout << "0th triplets with neighbors " << n0 << endl; 
    */    
    //=======================================
    
    //collect track candidates
    
    map<int, int >  stamap;  stamap.clear();
    
    TStopwatch c_time_fit;
    
    int min_level = 0;//1; //remove pile-up in MAPS
    if (isec == -1) min_level = NStation-3 - 3; //only the longest tracks
    
    for (int ilev = NStation-3; ilev >= min_level; ilev--){
      
      static vector<CbmL1Track> vtrackcandidate; vtrackcandidate.clear();
      
      //how many levels to check
      int nlevel = (NStation-2)-ilev+1;
      int ntracks = 0;
      for (vector<vector<CbmL1Triplet> >::reverse_iterator vtriprIt = vvtrip.rbegin();
	   vtriprIt != vvtrip.rend(); ++vtriprIt){
	for (vector<CbmL1Triplet>::iterator tripIt = vtriprIt->begin();
	     tripIt != vtriprIt->end(); ++tripIt){
	  //check the level
	  if (tripIt->level != ilev) continue;
	  
	  if (ilev == 0){ //remove pile-up -> collect only MAPS triplets
	    if ((tripIt->phitl)->iStation != 0) continue;
	  }


	  //check if all hits are not used => clear start
	  if ( ((tripIt->phitl)->used)||((tripIt->phitm)->used)||((tripIt->phitr)->used) ) continue;
	  
	  CbmL1Triplet *pt = &(*tripIt);
	  
	  //OK, start with the triplet
	  
	  CbmL1Track newtrack, currenttrack;
	  
	  newtrack.good   = currenttrack.good   = true;
	  newtrack.length = currenttrack.length = 3;
	  newtrack.chi2   = currenttrack.chi2   = 1000.0;
	  newtrack.NDF    = currenttrack.NDF    = 0;
	  
	  newtrack.mass        = currenttrack.mass         = 0.1395679; // pion mass
	  newtrack.is_electron = currenttrack.is_electron  = false; // pion mass
	  
	  newtrack.StsHits.clear();                  currenttrack.StsHits.clear();
	  newtrack.StsHits.push_back(tripIt->phitl); currenttrack.StsHits.push_back(tripIt->phitl);
	  newtrack.StsHits.push_back(tripIt->phitm); currenttrack.StsHits.push_back(tripIt->phitm);
	  newtrack.StsHits.push_back(tripIt->phitr); currenttrack.StsHits.push_back(tripIt->phitr);
	  newtrack.TrdHits.clear();                  currenttrack.TrdHits.clear();
	  
	  //newtrack.Segments.clear();               currenttrack.Segments.clear();
	  newtrack.Triplets.clear();                 currenttrack.Triplets.clear();
	  newtrack.Triplets.push_back(pt);           currenttrack.Triplets.push_back(pt);
	  
	  newtrack.T[0] = currenttrack.T[0]  = tripIt->T[0];
	  newtrack.T[1] = currenttrack.T[1]  = tripIt->T[1];
	  newtrack.T[2] = currenttrack.T[2]  = tripIt->T[2];
	  newtrack.T[3] = currenttrack.T[3]  = tripIt->T[3];
	  newtrack.T[4] = currenttrack.T[4]  = tripIt->T[4];
	  newtrack.T[5] = currenttrack.T[5]  = tripIt->T[5];
	  
	  double INF = 100.0;
	  {
	    for (int i = 0; i < 15; i++) 
	      newtrack.C[i] = currenttrack.C[i]  = 0.0;
	  }
	  newtrack.C[ 0] = currenttrack.C[ 0] = INF;//tripIt->C[ 0];
	  newtrack.C[ 2] = currenttrack.C[ 2] = INF;//tripIt->C[ 2];
	  newtrack.C[ 5] = currenttrack.C[ 5] = INF;//tripIt->C[ 5];
	  newtrack.C[ 9] = currenttrack.C[ 9] = INF;//tripIt->C[ 9];
	  newtrack.C[14] = currenttrack.C[14] = INF;//tripIt->C[14];
	  
	  CbmL1Triplet *ptrip = &(*tripIt);
	  
	  currenttrack.chi2   = 0.0;
	  
	  //recalculate the current track using the Kalman filter (add the last hit) !!!
	  	  
	  // target constraint !!!
	  if (isec <= 1){//only for semi-primary tracks
	    currenttrack.Propagate( target_hit.z );
	    target_hit.Filter( currenttrack );
	    //currenttrack.NDF = 0;//restore NDF
	  }
	  
	  pt->phitl->Filter( currenttrack, 1 );

	  if ((pt->phitl)->iStation == 2) KF.vPipe[2].Pass( currenttrack, 1); //pipe between 2 and 3 
 	  pt->phitm->Filter( currenttrack, 1 );

	  if ((pt->phitm)->iStation == 2) KF.vPipe[2].Pass( currenttrack, 1 ); //pipe between 2 and 3
	  pt->phitr->Filter( currenttrack, 1 );

	  //SG if ((pt->phitr)->iStation == 2) KF.vPipe[2].Pass( currenttrack, 1 ); //pipe between 2 and 3
	  
	  CAFindTrack(ptrip, newtrack, currenttrack);
	  if (newtrack.NDF > 0){
	    newtrack.chi2 /= newtrack.NDF; //normalize
	  }
	  
	  
	  if (newtrack.length < ilev+2) newtrack.good = false; // maximum one hit missing
	  
	  if (newtrack.chi2 > TRACK_CHI2_CUT) newtrack.good = false;
	  
	  if (1){//suppress ghost

	    CbmL1StsHit *pmh = *((newtrack.StsHits).begin());
	    int ista = pmh->iStation;

	    double chi2 = 0.0;
	    if (newtrack.NDF > 0)
	      chi2 = newtrack.chi2/newtrack.NDF;
	    
	    //if (newtrack.length == 3) newtrack.good = false; // too short track
	    if ((newtrack.length == 3)&&(ista != 0)) newtrack.good = false; // too short non-MAPS track
	    if ((newtrack.length == 3)&&(ista == 0)&&(chi2>3.0)) newtrack.good = false; // bad short MAPS track
	    
	    if ((newtrack.length == 4)&&((ista==1)||(ista==2)||(ista==3))&&(chi2>3.0))
	      newtrack.good = false; 
	    
	    //if ((newtrack.length == 4)&&((ista==1)||(ista==2)||(ista==3))&&(fabs(1.0/newtrack.T[4]) < 0.3))
	    if (((newtrack.length == 4)||(newtrack.length == 5))&&((ista==1)||(ista==2))&&(fabs(1.0/newtrack.T[4]) < 0.3))
	      newtrack.good = false; 
	  }
	  
	  
	  if (newtrack.good){
	    vtrackcandidate.push_back(newtrack); //store track
	    ntracks++;
	  }
	}
	if (--nlevel == 0) break;
      }
      
      //cout <<"Level "<< ilev <<" track candidates "<< ntracks << endl;
      //cout << " Total number of track candidates found " << vtrackcandidate.size() <<endl;
      
      
      //make competition between tracks of the same length

      // 1. sort tracks
      sort(vtrackcandidate.begin(), vtrackcandidate.end(), CbmL1Track::compareChi2);
      
      ntracks = 0;
      
      for (vector<CbmL1Track>::iterator trIt = vtrackcandidate.begin();
	   trIt != vtrackcandidate.end(); ++trIt){
	//count number of used hits
	int nused = 0;
	{for (vector<CbmL1StsHit*>::iterator phIt = trIt->StsHits.begin();
	      phIt != trIt->StsHits.end(); ++phIt){
	  if ((*phIt)->used) nused++; 
	}}
	
	if (nused != 0) continue;

	//#ifdef XXX	
	//=======================================================
 	//gather MAPS hits using the Kalman filter

	vector<CbmL1StsHit*>::iterator i1 = trIt->StsHits.begin();

	if ((*i1)->iStation == FIRSTCASTATION){ // fit upstream
	  
	  CbmL1Track* tr = &(*trIt); 

	  // use fitted momentum
	  	  
	  tr->chi2 = 0;
	  tr->NDF = 0;
	  tr->C[ 0] = INF; 
	  tr->C[ 1] = 0.; tr->C[ 2] = INF;
	  tr->C[ 3] = 0.; tr->C[ 4] = 0.; tr->C[ 5] = INF;
	  tr->C[ 6] = 0.; tr->C[ 7] = 0.; tr->C[ 8] = 0.; tr->C[ 9] = INF;
	  tr->C[10] = 0.; tr->C[11] = 0.; tr->C[12] = 0.; tr->C[13] =  0.; tr->C[14] = INF;
	  vector<CbmL1StsHit*>::reverse_iterator i = tr->StsHits.rbegin();
	  (*i)->Filter( *tr, 0 );

	  int istold = (*i)->iStation;
	  for(i++; i!=tr->StsHits.rend(); i++ )
	    {
	      int ist = (*i)->iStation;
	      for(int j=istold-1; j>ist; j--)
		{
		  if( j==2 ) KF.vPipe[2].Pass( *tr, 0 );		  
		  KF.vStsMaterial[j].Pass( *tr, 0 );
		}
	      if( ist==2 ) KF.vPipe[2].Pass( *tr, 0 );
	      
	      (*i)->Filter( *tr, 0 );
	      istold = ist;
	    }
	  //printf("new track ...\n");

	  for (int is = FIRSTCASTATION-1; is >= 0; is--){

	    //propagate
	    
	    double z_is = KF.vStsMaterial[is].z;         // z of detection plane

	    if( is==2 ) KF.vPipe[2].Pass( *tr, 0 );
	    tr->Propagate( z_is );
	    
	    double x_est   = tr->T[0];
	    double x_minus = tr->T[0] - 5.0*sqrt(tr->C[0]);
	    double x_plus  = tr->T[0] + 5.0*sqrt(tr->C[0]);
	    
	    double y_est   = tr->T[1];
	    double y_minus = tr->T[1] - 5.0*sqrt(tr->C[2]);
	    double y_plus  = tr->T[1] + 5.0*sqrt(tr->C[2]);

	    
	    bool maps_station = 0;
	    if( StsHitsStartIndex[is]< StsHitsStopIndex[is] ) maps_station = 1;
	    //  DE: this piece of code is replaced by the PDAF-based one

	    if( !UsePDAF || !maps_station ){		
	      double dist2_best = 1000.0;
	      int ih_best = -1;
	      for (int ih = StsHitsStartIndex[is]; ih <= StsHitsStopIndex[is]; ih++){
		double x = vStsHits[ih].x();
		double y = vStsHits[ih].y();
		//double z = vStsHits[ih].z();
		
		if ((x < x_minus) || (x > x_plus)) continue;
		if (y < y_minus) continue;
		if (y > y_plus ) break;
		
		if (vStsHits[ih].used) continue;

		double dist2 = (x_est-x)*(x_est-x) + (y_est-y)*(y_est-y);
		if (dist2 < dist2_best){
		  dist2_best = dist2;
		  ih_best = ih;
		}
	      }
	      if (ih_best >= 0)
		{
		  vStsHits[ih_best].Filter( *tr, 0 );
		  tr->StsHits.push_back(&(vStsHits[ih_best]));
		  tr->length++;
		}
	    }else{	    
	      // DE: PDAF-based track following:

	      double sizeX=x_plus-x_minus;
	      double sizeY=y_plus-y_minus;	    
	      //printf("CovXX=%E CovXY=%E CovYY=%E\n",tr->C[0],tr->C[1],tr->C[2]);
	      //printf("sizeX=%f sizeY=%f\n",sizeX,sizeY);
	    
	      std::vector<CbmL1StsHit*> vpValidatedHits;
	      std::vector<CbmKFStsHit*> vpValidatedMeasurements;
	      vpValidatedHits.clear();
	      vpValidatedMeasurements.clear();
	      for(int ih = StsHitsStartIndex[is]; ih <= StsHitsStopIndex[is]; ih++){
		double x = vStsHits[ih].x();
		double y = vStsHits[ih].y();
		if ((x < x_minus) || (x > x_plus)) continue;
		if (y < y_minus) continue;
		if (y > y_plus ) break;		
		if (vStsHits[ih].used) continue;
		vpValidatedHits.push_back(&(vStsHits[ih]));
		vpValidatedMeasurements.push_back(&(vStsHits[ih].KFStsHit));
	      }
	      if(vpValidatedHits.size()>0){
		int best_hit_idx=0;
		CbmKFStsHit::FilterPDAF( *tr, vpValidatedMeasurements, 
					  0, 0,sizeX,sizeY, best_hit_idx );
		tr->StsHits.push_back(vpValidatedHits[best_hit_idx]);
		tr->length++;		
	      }
	    }    
	  }
	  
	  // correct NDF value to number of fitted track parameters (straight line(4) or helix(5) )
	  
	  tr->NDF -= 5;
	  if (tr->NDF > 0){
	    tr->chi2 /= tr->NDF; //normalize
	  }	  
	}

	//============================================================
	//#endif //XXX

	{for (vector<CbmL1StsHit*>::iterator phitIt = trIt->StsHits.begin();
	      phitIt != trIt->StsHits.end(); ++phitIt){
	  //mark hits as used
	  (*phitIt)->used = true;

	  if (!(*phitIt)->isStrip) continue;

	  int ista = (*phitIt)->iStation;
	  
	  for (int ih = StsHitsStartIndex[ista]; ih <= StsHitsStopIndex[ista]; ih++){
	    if (vStsHits[ih].iSector != (*phitIt)->iSector) continue;
	    if ((vStsHits[ih].iStripF == (*phitIt)->iStripF)||(vStsHits[ih].iStripB == (*phitIt)->iStripB))
	      vStsHits[ih].used = true;
	  }
	}}
	
	trIt->isec = isec;
	
	//store track
	vRTracks.push_back(*trIt);
	ntracks++;
	
	// /*
	  int MC_TrackID = (*(trIt->StsHits.begin()))->MC_Point;
	  if (MC_TrackID >= 0){
	    MC_TrackID = vMCPoints[MC_TrackID].ID;
	    //check if the track is ok
	    bool ok = true;
	    {for (vector<CbmL1StsHit*>::iterator phIt = trIt->StsHits.begin();
		  phIt != trIt->StsHits.end(); ++phIt){
	      int MC_HitID = (*phIt)->MC_Point;
	      if (MC_HitID >= 0){
		MC_HitID = vMCPoints[MC_HitID].ID;
		if (MC_HitID != MC_TrackID) ok = false;
	      }
	    }}
	    
	    if (ok){
	      int sta = (*(trIt->StsHits.begin()))->iStation;
	      if(stamap.find(sta) == stamap.end())
		stamap.insert(pair<int, int>(sta, 1));
	      else{
		stamap[sta] += 1;
	      }
	    }
	  }
	  // */
      }
      
      //cout<< " tracks "<< ntracks <<endl;
    }//ilev
    
    /*
      cout << " sta : " ;
      for( map<int, int>::reverse_iterator staIt = stamap.rbegin(); staIt != stamap.rend(); staIt++ ){
      cout << staIt->second << " ";
      }
      cout << endl;
      */
    
    c_time_fit.Stop();    
    time_fit += double(c_time_fit.CpuTime());
  }
  
  //cout<< "Total number of tracks found " << vRTracks.size() <<endl;
  
  //==================================
  
  c_time.Stop();    
  cout<<"CA Track Finder: " << (double(c_time.CpuTime()))
      <<" (" << time_find << " + " << time_fit << ") s/ev"<<endl;
  
  CATime = (double(c_time.CpuTime()));
  
  //cout << "===> ... CA Track Finder    " << endl << endl;
  
}


void CbmL1::CAFindTrack(const CbmL1Triplet* ptrip, CbmL1Track& newtrack, CbmL1Track currenttrack){

  /****************************************************************
   *                                                              *
   *     The routine performs recusrsive search for tracks        *
   *                                                              *
   *     I. Kisel                                    06.03.05     *
   *                                                              *
   ***************************************************************/

  //  cout << "CA track finder " << ptrip->level << endl;

  //CbmKF &KF = *CbmKF::Instance();

  if (ptrip->level == 0){ // the last triplet -> check and store
    
    if ((currenttrack.length > newtrack.length)||
	((currenttrack.length == newtrack.length)&&(currenttrack.chi2 < newtrack.chi2))){
      
      newtrack.StsHits.clear();
      {for (vector<CbmL1StsHit*>::iterator pIt = currenttrack.StsHits.begin();
	   pIt != currenttrack.StsHits.end(); pIt++){
	newtrack.StsHits.push_back(*pIt);
      }}

      newtrack.Triplets.clear();
      for (vector<CbmL1Triplet*>::iterator ptIt = currenttrack.Triplets.begin();
	   ptIt != currenttrack.Triplets.end(); ptIt++){
	newtrack.Triplets.push_back(*ptIt);
      }

      {for (int i = 0; i <  6; i++) newtrack.T[i] = currenttrack.T[i];}
      {for (int i = 0; i < 15; i++) newtrack.C[i] = currenttrack.C[i];}

      currenttrack.NDF -= 5;
      newtrack.NDF      = currenttrack.NDF;

      newtrack.chi2   = currenttrack.chi2;
      newtrack.length = currenttrack.length;
      newtrack.good   = currenttrack.good;
    } // store track
  } // ptrip->level == 0
  else {// ptrip->level != 0

    //store hits of the current track
    vector<CbmL1StsHit*> StsHitsTmp; StsHitsTmp.clear();
    for (vector<CbmL1StsHit*>::iterator phIt = currenttrack.StsHits.begin();
	 phIt != currenttrack.StsHits.end(); phIt++){
      StsHitsTmp.push_back(*phIt);
    }
    
    //store triplets of the current track
    vector<CbmL1Triplet*> TripletsTmp; TripletsTmp.clear();
    for (vector<CbmL1Triplet*>::iterator ptrIt = currenttrack.Triplets.begin();
	 ptrIt != currenttrack.Triplets.end(); ptrIt++){
      TripletsTmp.push_back(*ptrIt);
    }
    
    int LengthTmp = currenttrack.length;
    int NDFTmp    = currenttrack.NDF;
    
    double TTmp[6], CTmp[15];
    {for (int i = 0; i <  6; i++) TTmp[i] = currenttrack.T[i];}
    {for (int i = 0; i < 15; i++) CTmp[i] = currenttrack.C[i];}
    
    double Chi2Tmp = currenttrack.chi2;
    
    int star = (ptrip->phitr)->iStation;
    //reverse order of vectors
    int vecindex = (NStation-1) - star;
    vector<CbmL1Triplet> *pvt;
    pvt = &vvtrip[vecindex];

    //cout << " (" << ptrip->last_triplet - ptrip->first_triplet << ") ";


    for (int index = ptrip->first_triplet; index < ptrip->last_triplet; ++index){ 
      CbmL1Triplet *pt; pt = &(*pvt)[index];
      
      if (ptrip->level != pt->level+1) continue;

      if (fabs(ptrip->T[4] - pt->T[4])/sqrt(ptrip->C[14]) > 10.0) //bad neighbour
	continue;


      //if (ptrip->stationl != pt->stationl-1) //well tested
      //cout << "CAFindTrack => *** Wrong station matching for neighbors ***" << endl;

      //no used hits allowed
      if (((pt->phitl)->used)||((pt->phitm)->used)||((pt->phitr)->used)){ //???

	//compare and store track
	if ((currenttrack.length > newtrack.length)||
	    ((currenttrack.length == newtrack.length)&&(currenttrack.chi2 < newtrack.chi2))){
	  
	  newtrack.StsHits.clear();
	  for (vector<CbmL1StsHit*>::iterator phiIt = currenttrack.StsHits.begin();
	       phiIt != currenttrack.StsHits.end(); phiIt++){
	    newtrack.StsHits.push_back(*phiIt);
	  }

	  newtrack.Triplets.clear();
	  for (vector<CbmL1Triplet*>::iterator ptripIt = currenttrack.Triplets.begin();
	       ptripIt != currenttrack.Triplets.end(); ptripIt++){
	    newtrack.Triplets.push_back(*ptripIt);
	  }

	  {for (int i = 0; i <  6; i++) newtrack.T[i] = currenttrack.T[i];}
	  {for (int i = 0; i < 15; i++) newtrack.C[i] = currenttrack.C[i];}
	  
	  currenttrack.NDF -= 5;
	  newtrack.NDF      = currenttrack.NDF;
	  
	  newtrack.chi2   = currenttrack.chi2;
	  newtrack.length = currenttrack.length;
	  newtrack.good   = currenttrack.good;
	} // store track
      }
      else{//add this triplet to the current track

	currenttrack.StsHits.clear();
	for (vector<CbmL1StsHit*>::iterator phitIt = StsHitsTmp.begin();
	     phitIt != StsHitsTmp.end(); phitIt++){
	  currenttrack.StsHits.push_back(*phitIt);
	}
	currenttrack.StsHits.push_back(pt->phitr);

	for (vector<CbmL1Triplet*>::iterator ptripIt = TripletsTmp.begin();
	     ptripIt != TripletsTmp.end(); ptripIt++){
	  currenttrack.Triplets.push_back(*ptripIt);
	}
	currenttrack.Triplets.push_back(pt);

	{for (int i = 0; i <  6; i++) currenttrack.T[i] = TTmp[i];}
	{for (int i = 0; i < 15; i++) currenttrack.C[i] = CTmp[i];}
	
	currenttrack.chi2 = Chi2Tmp;

	currenttrack.NDF  = NDFTmp;

	currenttrack.length = LengthTmp + 1;
    
	//recalculate the current track using the Kalman filter (add the last hit) !!!

	pt->phitr->Filter(currenttrack,1);
	//SG if ((pt->phitr)->iStation == 2) KF.vPipe[2].Pass( currenttrack, 1 ); //pipe between 2 and 3
	
	bool go_further = true;
	if (currenttrack.NDF > 5){
	  if (currenttrack.chi2 /(currenttrack.NDF-5) > TRACK_CHI2_CUT)
	    go_further = false;
	}
	//go_further = true;//cancel chi2 cut

	if (go_further)
	  CAFindTrack(pt, newtrack, currenttrack);

      }//good triplet added
    }//first_triplet --- last_triplet
  }//level != 0

  //cout << "Track finder finished " << endl;

  return;
}

//========================================================