//#include "L1Algo/L1Algo.h"
#include "TaskManager.h"
#include "L1AlgoInputSettings.h"
#include "InputDataArray.h"
#include "L1AlgoInputMCData.h"
#include "CbmL1Track.h"
#include "KFPVertex.h"
#include "L1Event.h"
#include "CbmKFTrackFitter.h"
#include "KFPTrackVector.h"


#include "L1Timer.h"
#include "TDHelper.h"
#include "TMath.h"
#include "TRandom.h"

#include <iostream>
#include <Vc/Vc>
using ::Vc::float_v;

TaskManager *TaskManager::fInstance = 0;

void TaskManager::Init( const L1AlgoInputSettings* settings, int verbose  ) {
  
  if( !fInstance ) fInstance = this;
  fVerbose = verbose;
  fSettings = settings;
  fTracker.Init( fSettings->GetSettings() );
  fTracker.fRadThick = fSettings->GetRadThickTable();

  fPerf.Init( "../Output/", &fTracker, fVerbose );
}

void TaskManager::SetData( const L1AlgoInputData* data, const L1AlgoInputMCData* mcData
                          , vector <float> &McDataHit,  vector <float> &McDataHit2 
                           ) {
  fData = data;
  fMCData = mcData;
  fMcDataHit2 = McDataHit2;
  fMcDataHit = McDataHit;
}

void TaskManager::SetData( const L1AlgoInputData* data, const L1AlgoInputMCData* mcData
//                           , vector <float> &McDataHit,  vector <float> &McDataHit2 
                           ) {
  fData = data;
  fMCData = mcData;
//       fMcDataHit2 = McDataHit2;
//     fMcDataHit = McDataHit;
}

void TaskManager::Run() {
    // set timers
  L1CATFIterTimerInfo timer;
  timer.Add("Total  ");
  timer.Add("TrackFinder");
  timer.Add("TrackFiter");
  timer.Add("PrimaryVertexFinder");
  timer.Add("SecondaryParticlesFinder");
  timer.Add("QualityCheck");
  static L1CATFIterTimerInfo stimer = timer;

  TStopwatch ts_all;
  TStopwatch ts;
  
//  L1Algo algoLocal;
//    algoLocal.Init(fSettings->GetSettings());
//    fTracker = algoLocal;
    // --------- Track Reconstruction -------------
  fTracker.SetData(
    fData->GetStsHits(), 
    fData->GetStsStrips(),
    fData->GetStsStripsB(),
    fData->GetStsZPos(),
    fData->GetSFlag(),
    fData->GetSFlagB(),
    fData->GetStsHitsStartIndex(),
    fData->GetStsHitsStopIndex() );
  if (fMCData) {
    fPerf.SetData(
      fMCData->GetHitStore(),
      fMCData->GetStsHits(),
      fMCData->GetMCPoints(),
      fMCData->GetMCTracks(),
      fMCData->GetHitMCRef()
    
      );
  }

 ts.Start(1);
  fTracker.CATrackFinder();
  ts.Stop();

  timer["TrackFinder"] = ts;
    
    // ------- Track Fit ---------
  ts.Start(1);
#if 1
  fTracker.L1KFTrackFitter(); // TODO
#else
  fTracker.KFTrackFitter_simple();
#endif
  ts.Stop();
  timer["TrackFiter"] = ts;
  
  
    // - Convert Tracks -
  vector<CbmL1Track> vRTracks;
  {
    vRTracks.clear();
    int start_hit = 0; // for interation in fTrackervRecoHits[]
    for (int i = 0; i < fTracker.NTracksIsecAll; i++){
      L1Track it = fTracker.vTracks[i];
      CbmL1Track t;
      
      for( int k=0; k<6; k++) t.T[k] = it.TFirst[k]; 
      for( int k=0; k<15; k++) t.C[k] = it.CFirst[k];
      for( int k=0; k<6; k++) t.TLast[k] = it.TLast[k];
      for( int k=0; k<15; k++) t.CLast[k] = it.CLast[k];
    
      t.chi2 = it.chi2;
      t.NDF = it.NDF;
      t.fTrackTime = it.fTrackTime;
      

      for( int k = 0; k < it.NHits; k++ ){ 
        t.StsHits.push_back( fTracker.vRecoHits[start_hit++]);
      }
      t.mass = 0.1395679; // pion mass
      t.is_electron = 0;
      
    // float prob = (TDHelper<float>::Chi2IProbability(  t.NDF, t.chi2 ));
      //float prob = (TMath::Prob(  t.chi2, t.NDF ));
     // if (prob < 0.004) continue;
      
      vRTracks.push_back(t);
      
    } // for vTracks
  }

  if (fMCData) {
    fPerf.SetRecoTracks(vRTracks);
//     fPerf.SetRecoEvents(&vEvent);
//     fPerf.SetPrimaryVertex(pv);
    fPerf.TrackMatch();
  }

  
  
//   for(int iTrack=0; iTrack<vRTracks.size() ; iTrack++)
//   {
//       float par[6] = {0.f};
//       float cov[15];
//       
//         
//       if( fPerf.GetRTracks()[iTrack].IsGhost() ) continue;
//       if( fPerf.GetRTracks()[iTrack].GetMaxPurity() < 0.7 ) continue;
//       if( !(fPerf.GetRTracks()[iTrack].GetMCTracks()[0]->IsPrimary()) ) continue;
// 
//     
//       int iPointInMc = fPerf.GetRTracks()[iTrack].GetMCTracks()[0]->Points[0];
// 
//       
//       float mcX  = fMCData->GetMCPoints()[iPointInMc].xIn;
//       float mcY  = fMCData->GetMCPoints()[iPointInMc].yIn;
//       float mcZ  = fMCData->GetMCPoints()[iPointInMc].zIn;
//       float mcPx = fMCData->GetMCPoints()[iPointInMc].pxIn;
//       float mcPy = fMCData->GetMCPoints()[iPointInMc].pyIn;
//       float mcPz = fMCData->GetMCPoints()[iPointInMc].pzIn;
//       float mcQP = fMCData->GetMCPoints()[iPointInMc].q/fMCData->GetMCPoints()[iPointInMc].p;
//       
//       par[0] = mcX + gRandom->Gaus(0, 0.001);
//       par[1] = mcY + gRandom->Gaus(0, 0.01);
//       par[2] = mcPx/mcPz + gRandom->Gaus(0, 0.005);
//       par[3] = mcPy/mcPz + gRandom->Gaus(0, 0.005);
//       par[4] = mcQP + gRandom->Gaus(0, 0.001);
//       par[5] = mcZ;
//       
//       for(int iC=0; iC<15; iC++)
//           cov[iC] = 0.f;
//       
// //       x, y, tx = px/pz, ty = py/pz, q/p, z
//       cov[0] = 0.001*0.001;
//       cov[2] = 0.01*0.01;
//       cov[5] = 0.005*0.005;
//       cov[9] = 0.005*0.005;
//       cov[14] = 0.001*0.001;
//       
//       for( int k=0; k<6; k++) vRTracks[iTrack].T[k] = par[k]; 
//       for( int k=0; k<15; k++) vRTracks[iTrack].C[k] = cov[k];
//   }
  
  vector < vector<int> >timeBins;
  vector <L1Event> vEvent;
      
  CbmKFTrackFitter fitter;
  vector<L1FieldRegion> vField;
  vector<float> chiToVtx;
  fitter.SetAlgo(&fTracker);
  fitter.SetHits(fData);
//   fitter.GetChiToVertex(fPerf.GetRTracks(), vField, chiToVtx, 1000000.f);
  fitter.GetChiToVertex(vRTracks, vField, chiToVtx, 5.f);
//   fitter.CalculateFieldRegion(vRTracks,vField);
    
  
  const float starttime = -100;
  const float stoptime = 1500000;
  int binstep,bini;
  int numberbins = 1500100;
  timeBins.resize(numberbins);
  binstep=(stoptime-starttime)/numberbins;
  for(unsigned int i=0; i<vRTracks.size(); i++)
  { 
//    cout<<vRTracks.size()<<" <vRTracks.size()"<<endl;
//    cout<<vRTracks[i].fTrackTime<<" vRTracks[i].fTrackTime "<<starttime<<" starttime "<<binstep<<" binstep"<<endl;
//     const vector< CbmL1MCTrack* > mcTracks = fPerf.GetRTracks()[i].GetMCTracks();
//     std::cout << "track " << i << " n mc " <<  fPerf.GetRTracks()[i].GetMCTracks().size() << std::endl;
//     if( fPerf.GetRTracks()[i].IsGhost() ) continue;    //cut for Primary Tracks
//     if( fPerf.GetRTracks()[i].GetMaxPurity() < 0.7 ) continue;
//     if( !(mcTracks[0]->IsPrimary()) ) continue; 
      
    bini =round((vRTracks[i].fTrackTime-starttime)/binstep);
    timeBins[bini].push_back(i);
  }
  
  L1Event event;
  vector <int> vKFPTrack;

  for(int k=1;k<numberbins-100;k++)
  {
    for(unsigned int l=0;l<timeBins[k].size();l++)
    {
      if(timeBins[k].size()>0)
      { 
        
        //int *timeTr =  vRTracks[timeBins[k][l]];
        vKFPTrack.push_back(timeBins[k][l]);
        // fKFPTrack.push_back(vKFPTracks[timeBins[k][l]]);
            
        for(unsigned int i=0;i<vRTracks[timeBins[k][l]].StsHits.size();i++)
        {
          vector <int>& hits = event.getHits();
          hits.push_back(vRTracks[timeBins[k][l]].StsHits[i]);
            // cout<<"hit "<<vRTracks[timeBins[k][l]].StsHits[i]<<endl;
        }
      }
    }
    
    int ntracks =0;
    bool stopEvent = 1;
    for(int iZero=0; iZero<7; iZero++)
    {
      stopEvent &= (timeBins[k+iZero].size()==0);
      ntracks+=timeBins[iZero].size();
    }

//      if( timeBins[k-1].size()>0 && timeBins[k].size()==0 && timeBins[k+1].size()==0 && timeBins[k+2].size()==0)
    if( timeBins[k-1].size()>0 && stopEvent && vKFPTrack.size()>3)
//     if( k==numberbins-101) 
    { 
      KFPTrackVector& tracks = event.getTracks();
      tracks.Resize(vKFPTrack.size());
      
      for(unsigned int iTr=0; iTr<vKFPTrack.size(); iTr++)
      {
        float par[6] ={0.f};
        double V[15] = {0.f};
        
        for (int i=0; i<15; i++) {V[i] = vRTracks[vKFPTrack[iTr]].C[i];}
        float a = vRTracks[vKFPTrack[iTr]].T[2], b = vRTracks[vKFPTrack[iTr]].T[3], qp = vRTracks[vKFPTrack[iTr]].T[4];
        
        float c2 = 1.f/(1.f + a*a + b*b);
        float pq = 1.f/qp;
        float p2 = pq*pq;
        float pz = sqrt(p2*c2);
        float px = a*pz;
        float py = b*pz;
        
        float H[3] = { -px*c2, -py*c2, -pz*pq };
        
//         int iPointInMc = fPerf.GetRTracks()[vKFPTrack[iTr]].GetMCTracks()[0]->Points[0];
// //         std::cout<<iPointInMc<<std::endl;
// //         float mcX  = fPerf.GetRTracks()[vKFPTrack[iTr]].GetMCTracks()[0]->x;
// //         float mcY  = fPerf.GetRTracks()[vKFPTrack[iTr]].GetMCTracks()[0]->y;
// //         float mcZ  = fPerf.GetRTracks()[vKFPTrack[iTr]].GetMCTracks()[0]->z;
//         
//         float mcX  = fMCData->GetMCPoints()[iPointInMc].xIn;
//         float mcY  = fMCData->GetMCPoints()[iPointInMc].yIn;
//         float mcZ  = fMCData->GetMCPoints()[iPointInMc].zIn;
//     
// //         std::cout << " mcTracks " <<fPerf.GetRTracks()[vKFPTrack[iTr]].GetMCTracks()[0]->x<<" "<<fPerf.GetRTracks()[vKFPTrack[iTr]].GetMCTracks()[0]->y<<" "<<fPerf.GetRTracks()[vKFPTrack[iTr]].GetMCTracks()[0]->z<<std::endl;
//         std::cout << " mcPoints " << mcX <<" "<< mcY << " "<< mcZ << std::endl;
// //         std::cout << " recoTrack " << vRTracks[vKFPTrack[iTr]].T[0] <<" "<< vRTracks[vKFPTrack[iTr]].T[1] << " " << vRTracks[vKFPTrack[iTr]].T[2] << std::endl;
//        
//         float mcPx = fMCData->GetMCPoints()[iPointInMc].pxIn;
//         float mcPy = fMCData->GetMCPoints()[iPointInMc].pyIn;
//         float mcPz = fMCData->GetMCPoints()[iPointInMc].pzIn;
//         
//         par[0] = mcX + gRandom->Gaus(0, 0.001);
//         par[1] = mcY + gRandom->Gaus(0, 0.01);
//         par[2] = mcZ;
//         par[3] = mcPx + gRandom->Gaus(0, 0.001);
//         par[4] = mcPy + gRandom->Gaus(0, 0.001);
//         par[5] = mcPz + gRandom->Gaus(0, 0.001);
//         
//         float cov[21];
//         for(int iC=0; iC<21; iC++)
//           cov[iC] = 0.f;
// 
//         cov[0] = 0.001*0.001;
//         cov[2] = 0.01*0.01;
// //         cov[5] = 0.05*0.05;
//         cov[9] = 0.001*0.001;
//         cov[14] = 0.001*0.001;
//         cov[20] = 0.001*0.001;
        
        
        par[0] = vRTracks[vKFPTrack[iTr]].T[0];
        par[1] = vRTracks[vKFPTrack[iTr]].T[1];
        par[2] = vRTracks[vKFPTrack[iTr]].T[5];
        par[3] = px;
        par[4] = py;
        par[5] = pz;
        
        float cxpz = H[0]*V[ 3] + H[1]*V[ 6] + H[2]*V[10];
        float cypz = H[0]*V[ 4] + H[1]*V[ 7] + H[2]*V[11];
        float capz = H[0]*V[ 5] + H[1]*V[ 8] + H[2]*V[12];
        float cbpz = H[0]*V[ 8] + H[1]*V[ 9] + H[2]*V[13];
        float cpzpz = H[0]*H[0]*V[5] +H[1]*H[1]*V[9] + H[2]*H[2]*V[14] 
        + 2*( H[0]*H[1]*V[8] +H[0]*H[2]*V[12] +H[1]*H[2]*V[13]);
        
        float cov[21];
        cov[ 0] = V[0];
        cov[ 1] = V[1];
        cov[ 2] = V[2];
        cov[ 3] = 0.f;
        cov[ 4] = 0.f;
        cov[ 5] = 0.f;
        cov[ 6] = V[3]*pz + a*cxpz; 
        cov[ 7] = V[4]*pz + a*cypz; 
        cov[ 8] = 0.f;
        cov[ 9] = V[5]*pz*pz + 2.f*a*pz*capz + a*a*cpzpz;
        cov[10] = V[6]*pz+b*cxpz; 
        cov[11] = V[7]*pz+b*cypz; 
        cov[12] = 0.f;
        cov[13] = V[8]*pz*pz + a*pz*cbpz + b*pz*capz + a*b*cpzpz;
        cov[14] = V[9]*pz*pz + 2.f*b*pz*cbpz + b*b*cpzpz;
        cov[15] = cxpz; 
        cov[16] = cypz; 
        cov[17] = 0.f;
        cov[18] = capz*pz + a*cpzpz;
        cov[19] = cbpz*pz + b*cpzpz;
        cov[20] = cpzpz;
        
        int q = 0;
        if(qp>0.f)
          q = 1;
        if(qp<0.f)
          q=-1;
        
        float field[10];
        int entrSIMD = iTr % float_vLen;
        int entrVec  = iTr / float_vLen;
        field[0] = vField[entrVec].cx0[entrSIMD];
        field[1] = vField[entrVec].cx1[entrSIMD];
        field[2] = vField[entrVec].cx2[entrSIMD];
        field[3] = vField[entrVec].cy0[entrSIMD];
        field[4] = vField[entrVec].cy1[entrSIMD];
        field[5] = vField[entrVec].cy2[entrSIMD];
        field[6] = vField[entrVec].cz0[entrSIMD];
        field[7] = vField[entrVec].cz1[entrSIMD];
        field[8] = vField[entrVec].cz2[entrSIMD];
        field[9] = vField[entrVec].z0[entrSIMD];
        
        for(Int_t iP=0; iP<6; iP++){
          tracks.SetParameter(par[iP], iP, iTr);}
        for(Int_t iC=0; iC<21; iC++){
          tracks.SetCovariance(cov[iC], iC, iTr);}
        for(Int_t iF=0; iF<10; iF++){
          tracks.SetFieldCoefficient(field[iF], iF, iTr);}
        tracks.SetId(vKFPTrack[iTr],iTr);
        tracks.SetPDG(-1, iTr);
        tracks.SetQ(q, iTr);
        tracks.SetPVIndex(-1, iTr);
      }
//       for(int mm=0;mm<10;mm++){
//         cout<<"FieldCoefficient    "<<event.getTracks().FieldCoefficient(0)<<endl;//}
            
      vEvent.push_back(event);
      event.Clear();
      vKFPTrack.clear();
    }
  }
  for(unsigned int iEv=0; iEv<vEvent.size();iEv++)
  {
    vEvent[iEv].TopoReconstructor(); 
  }
   
   
//   vector<KFMCTrack> mcTracks(nMCTracks);
//   for(Int_t iMC=0; iMC<nMCTracks; iMC++)
//   {
//     CbmMCTrack *cbmMCTrack = (CbmMCTrack*)fMCTrackArray->At(iMC);
// 
//     
//     mcTracks[iMC].SetX ( cbmMCTrack->GetStartX() );
//     mcTracks[iMC].SetY ( cbmMCTrack->GetStartY() );
//     mcTracks[iMC].SetZ ( cbmMCTrack->GetStartZ() );
//     mcTracks[iMC].SetPx( cbmMCTrack->GetPx() );
//     mcTracks[iMC].SetPy( cbmMCTrack->GetPy() );
//     mcTracks[iMC].SetPz( cbmMCTrack->GetPz() );
//     
//     Int_t pdg = cbmMCTrack->GetPdgCode();
//     Double_t q=1;
//     if ( pdg < 9999999 && ( (TParticlePDG *)TDatabasePDG::Instance()->GetParticle(pdg) ))
//       q = TDatabasePDG::Instance()->GetParticle(pdg)->Charge()/3.0;
//     else    q = 0;    
//     Double_t p = cbmMCTrack->GetP();
//     
//     mcTracks[iMC].SetMotherId(cbmMCTrack->GetMotherId());
//     mcTracks[iMC].SetQP(q/p);
//     mcTracks[iMC].SetPDG(pdg);
//     mcTracks[iMC].SetNMCPoints(0);
//   }
  
      
cout<<"Number of Events "<<vEvent.size()<<endl;

#if 0
    // ----------- Particle Reconstruction -------------
    // select tracks
  vector<CbmL1Track> vSelectedRTracks;
  for (unsigned i = 0; i < vRTracks.size(); i++){
    const CbmL1Track &t = vRTracks[i];
    
    bool ok = 1;
 
    for( int k=0; k<6; k++) ok = ok && finite(t.T[k]);
    for( int k=0; k<15; k++) ok = ok && finite(t.C[k]);
    ok = ok && finite(t.chi2) && finite(t.NDF);
    ok = ok && t.C[0]>0 && t.C[2]>0 && t.C[5]>0 && t.C[9]>0 && t.C[14]>0;
    
    if (!ok) continue;
    vSelectedRTracks.push_back(t);
  }

    // primary vertex
  CbmKFVertex pv;
  fParticles.clear();

  fKFParticleFinder.SetHits(fData);
  fKFParticleFinder.SetAlgo(&fTracker);
  
  ts.Start(1);
  fKFParticleFinder.ConstructPV(vSelectedRTracks,pv);
  ts.Stop();
  timer["PrimaryVertexFinder"] = ts;
//   std::cout << " Primary vertex " << pv.GetRefX() << " " << pv.GetRefY() << " " << pv.GetRefZ() << std::endl;
  
    // secondary particles
  ts.Start(1);
  fKFParticleFinder.FindParticles(vSelectedRTracks,fParticles,pv);
  ts.Stop();
  timer["SecondaryParticlesFinder"] = ts;
#else
  KFPVertex pv;
#endif
    // ------------- QA -------------
  ts.Start(1);
  if (fMCData) {
//     fPerf.SetRecoTracks(vRTracks);
    fPerf.SetRecoEvents(&vEvent);
    fPerf.SetPrimaryVertex(pv);
    fPerf.RunEv();
    fPerf.FillHist();
  }
  ts.Stop();
  timer["QualityCheck"] = ts;

  ts_all.Stop();
  timer["Total  "] = ts_all;

  if ( fVerbose ) {
    static int NEvents = 0;
    NEvents++;
    stimer += timer;
    if ( NEvents % 100 == 0 ) {
      cout << " Timers, ms " << endl;
      L1CATFIterTimerInfo tmp_timer = stimer/0.001/NEvents; // ms
      tmp_timer.PrintReal( 1 );
    }
  }
}

void TaskManager::PrintEff() {
  fPerf.PrintEff();
}