/*
 *=====================================================
 *
 *  CBM Level 1 Reconstruction 
 *  
 *  Authors: I.Kisel,  S.Gorbunov
 *
 *  e-mail : ikisel@kip.uni-heidelberg.de 
 *
 *=====================================================
 *
 *  Fit reconstructed tracks and find primary vertex
 *
 */

#include "CbmL1.h"
//#include "CbmL1TimeCounter.h"
#include "TStopwatch.h"

#include "CbmKF.h"
#include "CbmKFMath.h"
#include "CbmKFPrimaryVertexFinder.h"

void CbmL1::TrackFitter( vector<CbmL1Track> &Tracks, CbmL1Vtx *V )
{
  //Counter<long> m_time;
  //TimeCounter c_time(&m_time);

  TStopwatch timer;
  timer.Start();

  //int nt=0;
  for( vector<CbmL1Track>::iterator i = Tracks.begin(); i!=Tracks.end(); ++i)
    {
      CbmL1Track &T = *i;
      
      int stmin = 1000, stmax = -1000;
      for ( vector<CbmL1StsHit*>::iterator j = T.StsHits.begin(); j != T.StsHits.end(); ++j)
	{      
	  if ((*j)->iStation < stmin ) stmin = (*j)->iStation;
	  if ((*j)->iStation > stmax ) stmax = (*j)->iStation;
	}

      T.nStations = stmax - stmin + 1;
      T.is_long = ( T.StsHits.size() >= 4 ) && ( T.nStations >=4 );

      //T.is_long = T.is_long &&( vMCPoints[T.MapsHits.back()->MC_Point].mother_ID==-1 );
      //if ( !T.is_long ) continue;
      T.T[4]=0;
      for(int j=0; j<6; j++) T.TLast[j] = T.T[j];
      TInterfaceB TDown(&T);
      TDown.Fit( 1 );
      for(int j=0; j<6; j++) T.T[j]=T.TLast[j];
      T.Fit( 0 );
      for(int j=0; j<6; j++) T.TLast[j] = T.T[j];      
      TDown.Fit( 1 );
      for(int j=0; j<6; j++) T.T[j]=T.TLast[j];
      T.Fit( 0 );
    }

  stat_vtx_ntracks = 0; 

  if(V){ // primary vertex fit  
    
    vector<CbmKFTrackInterface*> vTracks;
    
   { for( vector<CbmL1Track>::iterator i = Tracks.begin(); i!=Tracks.end(); ++i)
      {
	if( ( i->StsHits.size() < 5 ) || ( i->nStations <5 ) ) continue;
	vTracks.push_back( &*i );    
	stat_vtx_ntracks ++;
      }
    }  
   cout<<"Fit vertex"<<endl;
    if ( vTracks.size()>=3 )
      {
	CbmKFPrimaryVertexFinder PVFinder;
	PVFinder.SetTracks( vTracks );
	PVFinder.Fit(*V);
      }
  }// prim. vtx 

  //c_time.Stop();    
  //stat_fit_time += double(c_time.Time());
  timer.Stop();
  stat_fit_time += timer.CpuTime();
  stat_fit_time_ntracks += Tracks.size();
}