// $Id: AliHLTTPCCAGBTracker.cxx,v 1.12 2010/09/01 10:38:27 ikulakov Exp $
// **************************************************************************
// This file is property of and copyright by the ALICE HLT Project          *
// ALICE Experiment at CERN, All rights reserved.                           *
//                                                                          *
// Primary Authors: Sergey Gorbunov <sergey.gorbunov@kip.uni-heidelberg.de> *
//                  Ivan Kisel <kisel@kip.uni-heidelberg.de>                *
//                  for The ALICE HLT Project.                              *
//                                                                          *
// Developed by:   Igor Kulakov <I.Kulakov@gsi.de>                          *
//                 Maksym Zyzak <M.Zyzak@gsi.de>                            *
//                                                                          *
// Permission to use, copy, modify and distribute this software and its     *
// documentation strictly for non-commercial purposes is hereby granted     *
// without fee, provided that the above copyright notice appears in all     *
// copies and that both the copyright notice and this permission notice     *
// appear in the supporting documentation. The authors make no claims       *
// about the suitability of this software for any purpose. It is            *
// provided "as is" without express or implied warranty.                    *
//                                                                          *
//***************************************************************************



#include "AliHLTTPCCAGBTracker.h"
#include "AliHLTTPCCAGBHit.h"
#include "AliHLTTPCCAGBTrack.h"
#include "AliHLTTPCCATrackParam.h"
#include "AliHLTArray.h"
#include "AliHLTTPCCAMath.h"
#include "AliHLTTPCCATrackLinearisationVector.h"
#include "AliHLTTPCCAPerformance.h"
#include "TStopwatch.h"

#include "ITSCATarget.h"
#include "ITSCAHits.h"
#include "ITSCAHitsV.h"
#include "ITSCASingletsV.h"
#include "ITSCADoubletsV.h"
#include "ITSCATriplets.h"
#include "ITSCATripletsV.h"
#include "ITSCATracks.h"

#include <algorithm>
#include <fstream>
#include <iostream>
using namespace std;

//TODO DELL ME!!!
#include "AliHLTTPCCAPerformance.h"
#include "AliHLTTPCCAMCPoint.h"
#include "AliHLTTPCPerformanceBase.h"

#ifdef DRAW
#include "AliHLTTPCCADisplay.h"
#define MAIN_DRAW
//#define DRAW_FIT
#define DRAW_CA
#endif //DRAW

  // debug option
//#define USE_IDEAL_TF
#define USE_CA_FIT

bool SINGLE_THREADED = false;

AliHLTTPCCAGBTracker::AliHLTTPCCAGBTracker()
    :
    fHits( 0 ),
    fNHits( 0 ),
    fTrackHits( 0 ),
    fTracks( 0 ),
    fNTracks( 0 ),
    fTime( 0 ),
    fStatNEvents( 0 ),
    fSliceTrackerTime( 0 ),
    fSliceTrackerCpuTime( 0 )
{
  //* constructor
  for ( int i = 0; i < 20; i++ ) fStatTime[i] = 0;
}

void AliHLTTPCCAGBTracker::Init()
{
  fNHits = 0;
  fTrackHits = 0;
  fTracks = 0;
  fNTracks = 0;
  fTime = 0.;
  fStatNEvents = 0;
  fSliceTrackerTime = 0.;
  fSliceTrackerCpuTime = 0.;
  for ( int i = 0; i < 20; ++i ) {
    fStatTime[i] = 0.;
  }
}

AliHLTTPCCAGBTracker::~AliHLTTPCCAGBTracker()
{
  //* destructor
  StartEvent();
}

void AliHLTTPCCAGBTracker::StartEvent()
{
  //* clean up track and hit arrays

  delete[] fTrackHits;
  fTrackHits = 0;
  delete[] fTracks;
  fTracks = 0;
  fNHits = 0;
  fNTracks = 0;
}


void AliHLTTPCCAGBTracker::SetNHits( int nHits )
{
  //* set the number of hits
  fHits.Resize( nHits );
  fNHits = nHits;
}

void AliHLTTPCCAGBTracker::IdealTrackFinder()
{
  AliHLTTPCCAPerformance* perf = &AliHLTTPCCAPerformance::Instance();

  if (fHits.Size() > 0)
    sort( &(fHits[0]), &(fHits[fHits.Size()-1]), AliHLTTPCCAGBHit::Compare );
  
  const int NMCTracks = perf->GetMCTracks()->Size();
  vector<vector<int> > hits(NMCTracks+1);

  for(int iH=0; iH<fHits.Size(); iH++)
  {
    int id = fHits[iH].ID();
    int trackId = perf->HitLabel(id).fLab[0];
    if(trackId < 0) trackId = NMCTracks;
/*std::cout << trackId<< " " << NMCTracks << std::endl;*/
    hits[trackId].push_back(iH);
  }

  int nTracks = 0;
  for(int iT=0; iT<NMCTracks+1; iT++)
    if(hits[iT].size()>0) nTracks++;

  if(fTracks) delete [] fTracks;
  fTracks = new AliHLTTPCCAGBTrack[nTracks];
  fNTracks = nTracks;

  if(fTrackHits) delete [] fTrackHits;
  fTrackHits = new int[fHits.Size()];

  int curHit = 0;
  int curTr = 0;
  for(int iT=0; iT<NMCTracks+1; iT++)
  {
    if(hits[iT].size() ==0) continue;

    fTracks[curTr].SetNHits( hits[iT].size() );
    fTracks[curTr].SetFirstHitRef( curHit );

    for(unsigned int iH=0; iH<hits[iT].size(); iH++)
    {
      fTrackHits[curHit] = hits[iT][iH];
      curHit++;
    }
    curTr++;
  }

}

inline void ConvertTrackParamToVector( AliHLTTPCCATrackParam t0[ushort_v::Size],
                                       AliHLTTPCCATrackParamVector &t, int &nTracksV)
{
  sfloat_v tmpVec;
  short_v tmpVecShort;
  sfloat_v::Memory tmpFloat;
  short_v::Memory tmpShort;

  for(int iV=0; iV < nTracksV; iV++) tmpFloat[iV] = t0[iV].X();
  tmpVec.load( tmpFloat );
  t.SetX(tmpVec);
  for(int iV=0; iV < nTracksV; iV++) tmpFloat[iV] = t0[iV].SignCosPhi();
  tmpVec.load( tmpFloat );
  t.SetSignCosPhi(tmpVec);

  for(int iP=0; iP<5; iP++)
  {
    for(int iV=0; iV < nTracksV; iV++) tmpFloat[iV] = t0[iV].Par()[iP];
    tmpVec.load( tmpFloat );
    t.SetPar(iP,tmpVec);
  }
  for(int iC=0; iC<15; iC++)
  {
    for(int iV=0; iV < nTracksV; iV++) tmpFloat[iV] = t0[iV].Cov()[iC];
    tmpVec.load( tmpFloat );
    t.SetCov(iC,tmpVec);
  }
  for(int iV=0; iV < nTracksV; iV++) tmpFloat[iV] = t0[iV].Chi2();
  tmpVec.load( tmpFloat );
  t.SetChi2(tmpVec);
  for(int iV=0; iV < nTracksV; iV++) tmpShort[iV] = t0[iV].NDF();
  tmpVecShort.load( tmpShort );
  t.SetNDF(tmpVecShort);
}

void AliHLTTPCCAGBTracker::FitTracks()
{
//   for(int iTr=0; iTr<fNTracks; iTr+=ushort_v::Size)
//   {
//     int nTracksVector = ushort_v::Size;
//     if(fNTracks - iTr < ushort_v::Size )
//     nTracksVector = fNTracks - iTr;
  for(int iTr=0; iTr<fNTracks; iTr+=1)
  {
    int nTracksVector = 1;

    ushort_v::Memory nTrackHits;
    ushort_v::Memory memFirstHits;


    AliHLTTPCCATrackParam startPoint[ushort_v::Size];
    AliHLTTPCCATrackParam endPoint[ushort_v::Size];

    for(int iv=0; iv<nTracksVector; iv++)
    {
      nTrackHits[iv]   = fTracks[iTr+iv].NHits();
      memFirstHits[iv] = fTracks[iTr+iv].FirstHitRef();
      startPoint[iv]   = fTracks[iTr+iv].Param();
      endPoint[iv]     = startPoint[iv];
    }

#ifdef DRAW_FIT
    AliHLTTPCCAPerformance* perf = &AliHLTTPCCAPerformance::Instance();
    
    AliHLTTPCCADisplay::Instance().SetTPCView();
    AliHLTTPCCADisplay::Instance().DrawTPC();
    ushort_v nHitsDraw(nTrackHits);

    for(int ihit = 0; ihit<nHitsDraw.max(); ihit++)
    {
      for(int iV=0; iV<nTracksVector; iV++)
      {
        if( ihit > nTrackHits[iV] - 1) continue;
        const int jhit = ihit;
        AliHLTTPCCAGBHit &h = fHits[fTrackHits[memFirstHits[iV] + jhit]];
        AliHLTTPCCADisplay::Instance().DrawGBPoint(h.X(), h.Y(), h.Z(), h.GetAngle(),iV);
      }
    }

    AliHLTTPCCAGBHit &hh = fHits[fTrackHits[memFirstHits[0] ]];
    const AliHLTTPCPerformanceBase::AliHLTTPCCAHitLabel &l = perf->HitLabel( hh.ID() );
    const int MCIndex = l.fLab[0];
    if(MCIndex>-1)
    {
      AliHLTTPCCAMCTrack &mc = (*perf->GetMCTracks())[MCIndex];
      AliHLTTPCCALocalMCPoint mcPoint =  (*perf->GetMCPoints())[mc.FirstMCPointID()];
      double mcX0 =  mcPoint.X();
      double mcY0 =  mcPoint.Y();
      double mcZ  =  mcPoint.Z();
      AliHLTTPCCADisplay::Instance().DrawGBPoint((float)mcX0, (float)mcY0, (float)mcZ, (int)2, (Size_t)1);
    }
    AliHLTTPCCADisplay::Instance().Ask();
#endif

    AliHLTTPCCATrackParamVector vStartPoint;
    AliHLTTPCCATrackParamVector vEndPoint;
    ConvertTrackParamToVector(startPoint,vStartPoint,nTracksVector);

    sfloat_m active0 = static_cast<sfloat_m>( ushort_v( Vc::IndexesFromZero ) < nTracksVector );

    ushort_v firstHits(memFirstHits);

    sfloat_m fitted = sfloat_m(true);
    fitted &= static_cast<sfloat_m>(static_cast<ushort_v>(nTrackHits) > 0);
    
#ifdef USE_CA_FIT // fit only forward
#ifdef DRAW_FIT
      AliHLTTPCCADisplay::Instance().ClearView();
      AliHLTTPCCADisplay::Instance().SetTPCView();
      AliHLTTPCCADisplay::Instance().DrawTPC();
      
      AliHLTTPCCADisplay::Instance().DrawGBPoint(0,0,0, 2, 0.5); // target

      AliHLTTPCCADisplay::Instance().Ask();
#endif
    
    fitted &= FitTrackCA( vEndPoint, firstHits, nTrackHits, nTracksVector,active0, 0 );
#else

    InitialTrackApproximation( vStartPoint, firstHits, nTrackHits, nTracksVector, active0);
    for(int iTimes = 0; iTimes<1; iTimes++)
    {
      vEndPoint = vStartPoint;
        //refit in the forward direction: going from the first hit to the last, mask "fitted" marks with 0 tracks, which are not fitted correctly
      fitted &= FitTrack( vEndPoint, firstHits, nTrackHits, nTracksVector,active0, 0 );
#ifdef DRAW_FIT
      AliHLTTPCCADisplay::Instance().ClearView();
      AliHLTTPCCADisplay::Instance().SetTPCView();
      AliHLTTPCCADisplay::Instance().DrawTPC();

      for(int ihit = 0; ihit<nHitsDraw.max(); ihit++)
      {
        for(int iV=0; iV<nTracksVector; iV++)
        {
          if( ihit > nTrackHits[iV] - 1) continue;
          const int jhit = ihit;
          AliHLTTPCCAGBHit &h = fHits[fTrackHits[memFirstHits[iV] + jhit]];
          AliHLTTPCCADisplay::Instance().DrawGBPoint(h.X(), h.Y(), h.Z(), h.GetAngle(),iV);
        }
      }
      AliHLTTPCCADisplay::Instance().Ask();
#endif
      vStartPoint = vEndPoint;
        //refit in the backward direction: going from the last hit to the first
      fitted &= FitTrack( vStartPoint, firstHits, nTrackHits, nTracksVector,active0, 1 );
#ifdef DRAW_FIT
      AliHLTTPCCADisplay::Instance().ClearView();
      AliHLTTPCCADisplay::Instance().SetTPCView();
      AliHLTTPCCADisplay::Instance().DrawTPC();

      for(int ihit = 0; ihit<nHitsDraw.max(); ihit++)
      {
        for(int iV=0; iV<nTracksVector; iV++)
        {
          if( ihit > nTrackHits[iV] - 1) continue;
          const int jhit = ihit;
          AliHLTTPCCAGBHit &h = fHits[fTrackHits[memFirstHits[iV] + jhit]];
          AliHLTTPCCADisplay::Instance().DrawGBPoint(h.X(), h.Y(), h.Z(), h.GetAngle(),iV);
        }
      }
      AliHLTTPCCADisplay::Instance().Ask();
#endif
    }
#endif // USE_CA_FIT

    for(int iV=0; iV < nTracksVector; iV++)
    {
      startPoint[iV] = AliHLTTPCCATrackParam( vStartPoint, iV );
      endPoint[iV] = AliHLTTPCCATrackParam( vEndPoint, iV );
      if ( !fitted[iV] ) {
        startPoint[iV].SetAsInvalid();
        endPoint[iV].SetAsInvalid();
      }
    }

    for(int iV = 0; iV<nTracksVector; iV++)
    {
      AliHLTTPCCAGBTrack &trackGB = fTracks[iTr+iV];
      trackGB.SetInnerParam( startPoint[iV] );
      trackGB.SetOuterParam( endPoint[iV] );
      trackGB.SetDeDx( 0 );
    }

  }
}

void AliHLTTPCCAGBTracker::InitialTrackApproximation( AliHLTTPCCATrackParamVector &track,
                                                      ushort_v &firstHits,
                                                      ushort_v::Memory &NTrackHits,
                                                      int &nTracksV, sfloat_m active0 )
{
  ushort_v nHits(NTrackHits);
  nHits.setZero(static_cast<ushort_m>(!active0));
  sfloat_m good = nHits > 2;

  int nHitsMax = nHits.max();

  sfloat_v X(Vc::Zero), Y(Vc::Zero), R(Vc::Zero);
  sfloat_v lx(Vc::Zero), ly(Vc::Zero), lx2(Vc::Zero), ly2(Vc::Zero), lr2(Vc::Zero);
  sfloat_v x(Vc::Zero), y(Vc::Zero), 
           x2(Vc::Zero), y2(Vc::Zero), xy(Vc::Zero),
           r2(Vc::Zero), xr2(Vc::Zero), yr2(Vc::Zero), r4(Vc::Zero);

  sfloat_v::Memory xH0, yH0, zH0, angle0;
  for(int iV=0; iV < nTracksV; iV++)
  {
    if( !(active0[iV]) ) continue;
    AliHLTTPCCAGBHit &h = fHits[fTrackHits[firstHits[iV]]];

    xH0[iV] = h.X();
    yH0[iV] = h.Y();
    zH0[iV] = h.Z();
    angle0[iV] = h.Angle();
  }
  sfloat_v xTmp(xH0);
  sfloat_v yTmp(yH0);
  const sfloat_v angleV0(angle0);
  const sfloat_v sA = CAMath::Sin(angleV0);
  const sfloat_v cA = CAMath::Cos(angleV0);
  const sfloat_v xV0 = xTmp*cA + yTmp*sA;
  const sfloat_v yV0 =-xTmp*sA + yTmp*cA;
  const sfloat_v zV0(zH0);

  vector<sfloat_v> xV(nHitsMax), yV(nHitsMax);

  for ( unsigned short ihit = 0; ihit < nHitsMax; ihit++ ) {

    sfloat_m active = static_cast<sfloat_m>( ihit < nHits );
    if(active.isEmpty()) continue;
    sfloat_v::Memory xH, yH;

    for(int iV=0; iV < nTracksV; iV++)
    {
      if( !(active[iV]) ) continue;
      if( ihit > NTrackHits[iV] - 1) continue;
      AliHLTTPCCAGBHit &h = fHits[fTrackHits[firstHits[iV] + ihit]];

      xH[iV] = h.X();
      yH[iV] = h.Y();
    }

    xTmp.load(xH);
    yTmp.load(yH);

    xV[ihit] = xTmp*cA +  yTmp*sA;
    yV[ihit] =-xTmp*sA +  yTmp*cA;
  }

  for(unsigned short ihit = 0; ihit < nHitsMax; ihit++)
  {
    sfloat_m active = static_cast<sfloat_m>( ihit < nHits );
    if(active.isEmpty()) continue;

    lx  = xV[ihit] - xV0;
    ly  = yV[ihit] - yV0;
    lx2 = lx*lx;
    ly2 = ly*ly;
    lr2 = lx2+ly2;

    x(active)  += lx;
    y(active)  += ly;
    x2(active) += lx2;
    y2(active) += ly2;
    xy(active) += lx*ly;
    r2(active) += lr2;
    xr2(active)+= lx*lr2;
    yr2(active)+= ly*lr2;
    r4(active) += lr2*lr2;
  }

  x /= static_cast<sfloat_v>(nHits);
  y /= static_cast<sfloat_v>(nHits);
  xy /= static_cast<sfloat_v>(nHits);
  x2 /= static_cast<sfloat_v>(nHits);
  y2 /= static_cast<sfloat_v>(nHits);
  xr2 /= static_cast<sfloat_v>(nHits);
  yr2 /= static_cast<sfloat_v>(nHits);
  r2 /= static_cast<sfloat_v>(nHits);
  r4 /= static_cast<sfloat_v>(nHits);

  const sfloat_v Cxx   = x2 - x*x;
  const sfloat_v Cxy   = xy - x*y;
  const sfloat_v Cyy   = y2 - y*y;
  const sfloat_v Cxr2  = xr2 - x*r2;
  const sfloat_v Cyr2  = yr2 - y*r2;
  const sfloat_v Cr2r2 = r4 - r2*r2;

  const sfloat_v Q1 = Cr2r2*Cxy - Cxr2*Cyr2;
  const sfloat_v Q2 = Cr2r2*(Cxx-Cyy) - Cxr2*Cxr2 + Cyr2*Cyr2;
  const sfloat_v Phi = 0.5f * CAMath::ATan2(2.f*Q1, Q2);
  const sfloat_v SinPhi = CAMath::Sin(Phi);
  const sfloat_v CosPhi = CAMath::Cos(Phi);
  const sfloat_v Kappa = (SinPhi*Cxr2 - CosPhi*Cyr2)/Cr2r2;

  const sfloat_v Delta = SinPhi*x - CosPhi*y - Kappa*r2;

  const sfloat_v Rho = 2.f*Kappa / CAMath::Sqrt(sfloat_v(Vc::One) - 4.f*Delta*Kappa);
  const sfloat_v D = 2.f*Delta / ( sfloat_v(Vc::One) + CAMath::Sqrt(sfloat_v(Vc::One) - 4.f*Delta*Kappa));

  R = sfloat_v(Vc::One)/Rho;
  X = (D+R)*SinPhi;
  Y = -(D+R)*CosPhi;

  sfloat_v kappa(Vc::Zero);
  kappa(good) = Rho;

  const sfloat_v sinPhi0 = -X*kappa;
  const sfloat_v cosPhi0 = Y*kappa;

  sfloat_v dS(Vc::Zero), dS2(Vc::Zero), Z(Vc::Zero), dSZ(Vc::Zero);
  for ( unsigned short ihit = 0; ihit < nHitsMax; ihit++ ) {

    sfloat_m active = static_cast<sfloat_m>( ihit < nHits );
    if(active.isEmpty()) continue;
    sfloat_v::Memory zH;

    for(int iV=0; iV < nTracksV; iV++)
    {
      if( !(active[iV]) ) continue;
      if( ihit > NTrackHits[iV] - 1) continue;
      AliHLTTPCCAGBHit &h = fHits[fTrackHits[firstHits[iV] + ihit]];

      zH[iV] = h.Z();
    }
    const sfloat_v zV(zH);

    lx(active)  = xV[ihit]-xV0;
    ly(active)  = yV[ihit]-yV0;

    const sfloat_v ex = cosPhi0;
    const sfloat_v ey = sinPhi0;
    const sfloat_v dx = lx;

    sfloat_v ey1 = kappa * dx + ey;

  // check for intersection with X=x

    sfloat_v ex1 = CAMath::Sqrt( 1.f - ey1 * ey1 );
    ex1( ex < Vc::Zero ) = -ex1;

    const sfloat_v dx2 = dx * dx;
    const sfloat_v ss = ey + ey1;
    const sfloat_v cc = ex + ex1;

    const sfloat_v cci = 1.f / cc;
    const sfloat_v exi = 1.f / ex;
    const sfloat_v ex1i = 1.f / ex1;

    const sfloat_v tg = ss * cci; // tan((phi1+phi)/2)

    const sfloat_v dy = dx * tg;
    sfloat_v dl = dx * CAMath::Sqrt( 1.f + tg * tg );

    dl( cc < Vc::Zero ) = -dl;
    const sfloat_v dSin = CAMath::Max( sfloat_v( -1.f ), CAMath::Min( sfloat_v( Vc::One ), dl * kappa * 0.5f ) );
    const sfloat_v ds = ( CAMath::Abs( kappa ) > 1.e-4 )  ? ( 2 * CAMath::ASin( dSin ) / kappa ) : dl;

    dS(active)  += ds;
    dS2(active) += ds*ds;
    Z(active)   += zV;
    dSZ(active) += ds*zV;
  }

  sfloat_v det  = Vc::One/( static_cast<sfloat_v>(nHits)*dS2 - dS*dS );
  sfloat_v Z0(Vc::Zero), X0(Vc::Zero), Y0(Vc::Zero), dzds(Vc::Zero);
  Z0(good)   = det*( dS2*Z - dS*dSZ);
  dzds(good) = det*( static_cast<sfloat_v>(nHits)*dSZ - Z*dS );

  sfloat_v signCosPhi0(Vc::One);
  signCosPhi0(cosPhi0<Vc::Zero) = -Vc::One;

#ifdef DRAW_FIT
  AliHLTTPCCADisplay::Instance().ClearView();
  AliHLTTPCCADisplay::Instance().SetTPCView();
  AliHLTTPCCADisplay::Instance().DrawTPC();

  for(int ihit = 0; ihit<nHitsMax; ihit++)
  {
    for(int iV=0; iV < nTracksV; iV++)
    {
      sfloat_m active = static_cast<sfloat_m>( ihit < nHits );
      if( !(active[iV]) ) continue;
      if( ihit > NTrackHits[iV] - 1) continue;
      AliHLTTPCCAGBHit &h = fHits[fTrackHits[firstHits[iV] + ihit]];
      AliHLTTPCCADisplay::Instance().DrawGBPoint(h.X(), h.Y(), h.Z(), h.GetAngle(),iV);
    }
  }

  float xCL = (X+xV0)[0];
  float yCL = (Y+yV0)[0];
  float xC = xCL*cA[0] - yCL*sA[0];
  float yC = xCL*sA[0] + yCL*cA[0];
  float rad = fabs(R[0]);

  AliHLTTPCCADisplay::Instance().DrawArc(xC, yC, rad, -1, 1);
  AliHLTTPCCADisplay::Instance().DrawGBPoint(xC, yC, 0.f, 0.f,-1);
  AliHLTTPCCADisplay::Instance().Ask();
#endif

  track.SetAngle(angleV0);
  track.SetSinPhi(sinPhi0);
  track.SetSignCosPhi(signCosPhi0);
  track.SetX(xV0);
  track.SetY(yV0);
  track.SetZ(Z0);
  track.SetDzDs(dzds);
  track.SetQPt(kappa/GetParameters().cBz());
}

sfloat_m AliHLTTPCCAGBTracker::FitTrack( AliHLTTPCCATrackParamVector &t,
                                         ushort_v &firstHits,
                                         ushort_v::Memory &NTrackHits,
                                         int &nTracksV, sfloat_m active0, bool dir )
{
  AliHLTTPCCATrackParamVector::AliHLTTPCCATrackFitParam fitPar;

  AliHLTTPCCATrackLinearisationVector l( t );

  bool first = 1;

  t.CalculateFitParameters( fitPar );

  ushort_v nHits(NTrackHits);
  nHits.setZero(static_cast<ushort_m>(!active0));

  int nHitsMax = nHits.max();

  for ( unsigned short ihit = 0; ihit < nHitsMax; ihit++ ) {

    sfloat_m active = active0 && static_cast<sfloat_m>( ihit < nHits );
    if(active.isEmpty()) continue;
    sfloat_v::Memory xH, yH, yErrH, zH, zErrH, hitAlpha;
    sfloat_v::Memory memXOverX0, memXTimesRho;

    for(int iV=0; iV < nTracksV; iV++)
    {
      if( !(active[iV]) ) continue;
      if( ihit > NTrackHits[iV] - 1) continue;
      const unsigned short jhit = dir ? ( NTrackHits[iV] - 1 - ihit ) : ihit;
      AliHLTTPCCAGBHit &h = fHits[fTrackHits[firstHits[iV] + jhit]];

      hitAlpha[iV] =  h.Angle();

      xH[iV] = h.X();
      yH[iV] = h.Y();
      zH[iV] = h.Z();

      yErrH[iV] = h.Err2Y();
      zErrH[iV] = h.Err2Z();

      memXOverX0[iV] = GetParameters().GetXOverX0(h.IRow());
      memXTimesRho[iV] = GetParameters().GetXTimesRho(h.IRow());
    }

    const sfloat_v err2Y(yErrH), err2Z(zErrH);
    sfloat_v xV(xH);
    sfloat_v yV(yH);
    const sfloat_v zV(zH);
    const sfloat_v hitAlphaV(hitAlpha);
    const sfloat_v xOverX0(memXOverX0);
    const sfloat_v xTimesRho(memXTimesRho);

#ifdef DRAW_FIT
    for(int iV=0; iV<nTracksV; iV++)
    {
      if(!active[iV]) continue;
      AliHLTTPCCADisplay::Instance().DrawGBPoint((float)xV[iV], (float)yV[iV], (float)zV[iV],(int)4,(Size_t)1);
    }
#endif

    const sfloat_m &rotated = t.Rotate(  -t.Angle() + hitAlphaV, l, .999f, active);

    active &= rotated;
    if(active.isEmpty()) continue;

    const sfloat_v cA = CAMath::Cos( hitAlphaV );
    const sfloat_v sA = CAMath::Sin( hitAlphaV );
    const sfloat_v x0 = xV, y0 = yV;

    xV =  x0*cA +  y0*sA;
    yV = -x0*sA +  y0*cA;

    t.SetAngle(hitAlphaV, active);
    const sfloat_m &transported = t.TransportToXWithMaterial( xV, l, fitPar, xOverX0, xTimesRho, GetParameters().cBz(), 0.999f, active);
//std::cout << "y  " << yV << " " << t.Y() << std::endl;
//std::cout << "z  " << zV << " " << t.Z() << std::endl;

#ifdef DRAW_FIT
    sfloat_v xGl = t.X()*cos(t.Angle()) - t.Y()*sin(t.Angle());
    sfloat_v yGl = t.X()*sin(t.Angle()) + t.Y()*cos(t.Angle());

    for(int iV=0; iV<nTracksV; iV++)
    {
      if(!active[iV]) continue;
      AliHLTTPCCADisplay::Instance().DrawGBPoint(xGl[iV], yGl[iV], t.Z()[iV],-1,0.5);
    }
    AliHLTTPCCADisplay::Instance().Ask();
#endif

    active &= transported;
    if(active.isEmpty()) continue;
    if ( first ) {
      t.SetCov( 0, 10.f, active );
      t.SetCov( 1,  0.f, active );
      t.SetCov( 2, 10.f, active );
      t.SetCov( 3,  0.f, active );
      t.SetCov( 4,  0.f, active );
      t.SetCov( 5,  1.f, active );
      t.SetCov( 6,  0.f, active );
      t.SetCov( 7,  0.f, active );
      t.SetCov( 8,  0.f, active );
      t.SetCov( 9,  10.f, active );
      t.SetCov( 10,  0.f, active );
      t.SetCov( 11,  0.f, active );
      t.SetCov( 12,  0.f, active );
      t.SetCov( 13,  0.f, active );
      t.SetCov( 14,  10.f, active );
      t.SetChi2( 0.f, active);
      t.SetNDF( -5, static_cast<short_m>(active) );
      t.CalculateFitParameters( fitPar );
      t.SetAngle(sfloat_v( hitAlpha ));
      first = 0;
    }

    const sfloat_m &filtered = t.FilterWithMaterial(yV, zV, err2Y, err2Z, 0.999f,active);
    active &= filtered;
    if(active.isEmpty()) continue;

#ifdef DRAW_FIT
    xGl = t.X()*cos(t.Angle()) - t.Y()*sin(t.Angle());
    yGl = t.X()*sin(t.Angle()) + t.Y()*cos(t.Angle());
    for(int iV=0; iV<nTracksV; iV++)
    {
      if(!active[iV]) continue;
      AliHLTTPCCADisplay::Instance().DrawGBPoint(xGl[iV], yGl[iV], t.Z()[iV],1,0.5);
    }
    AliHLTTPCCADisplay::Instance().Ask();
#endif

  }
  t.SetQPt( sfloat_v(1.e-8f), CAMath::Abs( t.QPt() ) < 1.e-8f );

  sfloat_m ok = active0;

// if track has infinite partameters or covariances, or negative diagonal elements of Cov. matrix, mark it as fitted uncorrectly
    for ( unsigned char i = 0; i < 15; i++ ) ok &= CAMath::Finite( t.Cov(i) );
  for ( unsigned char i = 0; i <  5; i++ ) ok &= CAMath::Finite( t.Par(i) );
    ///  ok = ok && ( t.GetX() > 50 ); //this check is wrong! X could be < 50, when a sector is rotated!
  ok &= (t.Cov(0) > Vc::Zero) && (t.Cov(2) > Vc::Zero) && (t.Cov(5) > Vc::Zero) && (t.Cov(9) > Vc::Zero) && (t.Cov(14) > Vc::Zero);
  ok &= CAMath::Abs( t.SinPhi() ) < .999f;

  t.SetSignCosPhi( 1, ok && static_cast<sfloat_m>(l.CosPhi() >= Vc::Zero) );
  t.SetSignCosPhi(-1, ok && static_cast<sfloat_m>(l.CosPhi() < Vc::Zero) );
  
  return ok;
}

  // used to check fit in CA
sfloat_m AliHLTTPCCAGBTracker::FitTrackCA( AliHLTTPCCATrackParamVector &t,
                                         ushort_v &firstHits,
                                         ushort_v::Memory &NTrackHits,
                                         int &nTracksV, sfloat_m active0, bool dir )
{
  UNUSED_PARAM2(nTracksV, dir); // TODO clean me
  sfloat_m active = active0;
  
  ushort_v NTHits(NTrackHits);
  active &= NTHits >= 3;

  if (active.isEmpty()) return active;
  
  NTHits.setZero(static_cast<ushort_m>(!active));
  
#if 1 // check tracks
  const unsigned short NHitsMax = NTHits.max();
  const unsigned short FirstHit = 0; // check part of the track begining from x-th hit
  const bool AddLastHit = true;
#else // check tracklets
  const unsigned short NHitsMax = 2; // check x-lets fit
  const unsigned short FirstHit = 3; // check part of the track begining from x-th hit
  const bool AddLastHit = false;
#endif 
  active &= NTHits >= NHitsMax+FirstHit;
  
  vector<ITSCAHitV> hits( NHitsMax );

  for ( unsigned short ihit = 0+FirstHit; ihit < NHitsMax+FirstHit; ihit ++ ) {
    const ushort_m valid = ihit < NTHits;
    ushort_v::Memory id;

    const AliHLTTPCCAGBHit **hs = new const AliHLTTPCCAGBHit*[sfloat_v::Size];
    foreach_bit(unsigned short iV, valid) {
      id[iV] = firstHits[iV] + ihit;
      hs[iV] = &(fHits[fTrackHits[id[iV]]]);
    }
    hits[ihit-FirstHit] = ITSCAHitV( hs, ushort_v(id), static_cast<sfloat_m>(valid) );
  }
  
  const ITSCAHitV& hit0 = hits[0];

#if 0  // target + hit
    // ITSCATarget target( 0, 0, 0, 3, 10, 0 ); // target at 0 with 3 cm error
  fMaxInvMom = 1.f; // 0.1 GeV tracks
  ITSCATarget target( 0, 0, 0, 0.1, 10, fMaxInvMom/3.f, GetParameters().VtxFieldValue(), 0 ); // target at 0 with 3 cm error // 0.1 GeV tracks

  t.InitByTarget( target );
  t.SetAngle( hit0.Angle() );
  t.InitDirection( hit0.X0(), hit0.X1(), hit0.X2() );
  
  for ( unsigned short ihit = 0; ihit < NHitsMax; ihit++ ) {
#elif 0 // start from 2 hits initialization
  const ITSCAHitV& hit1 = hits[1];
  fMaxInvMom = 20.f; // 0.05 GeV tracks
  ITSCATarget target( 0, 0, 0, 10e10, 10e10, fMaxInvMom/3.f, GetParameters().VtxFieldValue(), 0 ); // target at 0 with 3 cm error

  t.InitByTarget( target );
  t.SetAngle( hit0.Angle() );
  sfloat_v dx = hit1.GX()-hit0.GX();
  sfloat_v dy = hit1.GY()-hit0.GY();
  sfloat_v dz = hit1.GZ()-hit0.GZ();
  const sfloat_v cA = CAMath::Cos( hit0.Angle() );
  const sfloat_v sA = CAMath::Sin( hit0.Angle() );
  
  t.InitDirection( dx*cA + dy*sA, -dx*sA + dy*cA, dz );
  t.SetErr2QPt( fMaxInvMom*fMaxInvMom/9.f );
  t.SetErr2Y( hit0.Err2Y() );
  t.SetErr2Z( hit0.Err2Z() );

  for ( unsigned short ihit = 1; ihit < NHitsMax; ihit++ ) {
#else // start from hit + target

  fMaxInvMom = 1.f; // 0.1 GeV tracks
  ITSCATarget target( 0, 0, 0, 0.1, 10, fMaxInvMom/3.f, GetParameters().VtxFieldValue(), 1 ); // target at 0 with 3 cm error // 0.1 GeV tracks

  t.InitByHit( hit0, GetParameters(), fMaxInvMom/3.f );
  t.InitDirection( hit0.X0(), hit0.X1(), hit0.X2() );

  t.AddTarget( target, active );

  for ( unsigned short ihit = 1; ihit < NHitsMax; ihit++ ) { 
#endif
    const ITSCAHitV& hit = hits[ihit];
    active &= t.Transport( hit, GetParameters(), active );
    if( AddLastHit || ihit != NHitsMax-1 ) // don't add last hit, thereby checking extrapolation
      active &= t.Filter( hit, GetParameters(), active );
#ifdef DRAW_FIT
    sfloat_v xGl = t.X()*cos(hit.Angle()) - t.Y()*sin(hit.Angle());
    sfloat_v yGl = t.X()*sin(hit.Angle()) + t.Y()*cos(hit.Angle());

    foreach_bit( unsigned short iV, active & hit.IsValid() ) {
      AliHLTTPCCADisplay::Instance().DrawGBPoint(hit.GX()[iV], hit.GY()[iV], hit.GZ()[iV], (int)4,(Size_t)1);
      AliHLTTPCCADisplay::Instance().DrawGBPoint(xGl[iV], yGl[iV], t.Z()[iV],-1,0.5);
        // cout << xGl[iV] << " " << yGl[iV] << " " << t.Z()[iV] << endl;
    }
    
    AliHLTTPCCADisplay::Instance().Ask();
#endif
  }
  
  t.SetQPt( sfloat_v(1.e-8f), CAMath::Abs( t.QPt() ) < 1.e-8f );

  sfloat_m ok = active;

    // if track has infinite partameters or covariances, or negative diagonal elements of Cov. matrix, mark it as fitted uncorrectly
  
  for ( unsigned char i = 0; i < 15; i++ ) ok &= CAMath::Finite( t.Cov(i) );
  for ( unsigned char i = 0; i <  5; i++ ) ok &= CAMath::Finite( t.Par(i) );
    ///  ok = ok && ( t.GetX() > 50 ); //this check is wrong! X could be < 50, when a sector is rotated!
  ok &= (t.Cov(0) > Vc::Zero) && (t.Cov(2) > Vc::Zero) && (t.Cov(5) > Vc::Zero) && (t.Cov(9) > Vc::Zero) && (t.Cov(14) > Vc::Zero);
  ok &= CAMath::Abs( t.SinPhi() ) < .999f;

  t.SetQPt( -t.QPt() ); // FIXME why have we invert sign??
  
  return ok;
}

void AliHLTTPCCAGBTracker::FindTracks()
{
  //* main tracking routine
  fTime = 0;
  fStatNEvents++;

#ifdef MAIN_DRAW
  AliHLTTPCCAPerformance::Instance().SetTracker( this );
  AliHLTTPCCADisplay::Instance().Init();
  AliHLTTPCCADisplay::Instance().SetGB( this );
  AliHLTTPCCADisplay::Instance().SetTPC( GetParameters() );
#endif //MAIN_DRAW
  
#ifdef MAIN_DRAW

//   AliHLTTPCCADisplay::Instance().SetTPCView();
// /**  AliHLTTPCCADisplay::Instance().DrawTPC();
//   AliHLTTPCCADisplay::Instance().DrawGBHits( *this, -1, 0.2, 1 );
//   AliHLTTPCCADisplay::Instance().SaveCanvasToFile( "Hits.pdf");
//   AliHLTTPCCADisplay::Instance().Ask();
// */
// //   AliHLTTPCCADisplay::Instance().DrawGBHits( *this, kRed, 0.2, 2 );
// //   AliHLTTPCCADisplay::Instance().SaveCanvasToFile( "Hits_b.pdf");
// //   AliHLTTPCCADisplay::Instance().Ask();
#endif //MAIN_DRAW

 cout << " NHits = " << fNHits << endl;

  TStopwatch timer1;
  TStopwatch timer2;

  /// \brief The necessary data is transfered to the track-finder
/// The necessary data is transfered to the track-finder
///Data is structured and saved by track-finders for each sector. 
///To speed up the process  in each row 2D-grid with the bin size
///inversely proportional to the number of hits in the row is introduced.
///Hits are sorted by grid bins and for each grid bin 1st  hit is found and saved. 
///Such data structure allows to quickly   find closest hits to the point with given
///coordinates, which is required while neighbours hits are searched and additional 
///hits are attached to segments.

  fSliceTrackerTime = 0;
  fSliceTrackerCpuTime = 0;
  fTime = 0;
  for ( int i = 0; i < 20; ++i ) {
    fStatTime[i] = 0.;
  }
  
#ifdef USE_TIMERS
  timer1.Start();
#endif /// USE_TIMERS

  AliHLTTPCCAPerformance::Instance().ShiftHitsToMC(); // dbg
  //AliHLTTPCCAPerformance::Instance().ResimulateHits(); // dbg
  
#ifdef USE_IDEAL_TF          
  IdealTrackFinder();
  FitTracks();
#else
  CATrackFinder();
  FitTracks();
#endif

#ifdef USE_TIMERS
  timer1.Stop();
  fStatTime[12] = timer1.RealTime();
#endif /// USE_TIMERS
  /// Read hits, row by row

  fSliceTrackerTime += timer1.RealTime();
  fSliceTrackerCpuTime += timer1.CpuTime();
  //fTime+=timerMerge.RealTime();
  //std::cout<<"Merge time = "<<timerMerge.RealTime()*1.e3<<"ms"<<std::endl;
  //std::cout<<"End CA merging"<<std::endl;
  fTime += timer1.RealTime();

// #ifdef MAIN_DRAW // tracks are shown in global performance
//   AliHLTTPCCADisplay::Instance().SetTPCView();
//   AliHLTTPCCADisplay::Instance().ClearView();
//   AliHLTTPCCADisplay::Instance().DrawTPC();
//   AliHLTTPCCADisplay::Instance().DrawGBHits( *this );

//   for( int iT = 0; iT < fNTracks; ++iT )
//     AliHLTTPCCADisplay::Instance().DrawGBTrack( iT, 1, 1 );
  
//   AliHLTTPCCADisplay::Instance().SaveCanvasToFile( "Hits.pdf");
//   AliHLTTPCCADisplay::Instance().Ask();

// #endif //DRAW
}

void AliHLTTPCCAGBTracker::WriteSettings( std::ostream &out ) const
{
  //* write settings to the file
  UNUSED_PARAM1(out); // TODO
//    out << fSlices[iSlice].Param();
}

void AliHLTTPCCAGBTracker::ReadSettings( std::istream &in )
{
  //* Read settings from the file
//  AliHLTTPCCAParam param;
  in >> fParameters;

//  fSlices[iSlice].Initialize( param );
}

void AliHLTTPCCAGBTracker::WriteEvent( FILE *file ) const
{
  // write event to the file

  const int nHits = NHits();
  int written = std::fwrite( &nHits, sizeof( int ), 1, file );
  assert( written == 1 );
  written = std::fwrite( fHits.Data(), sizeof( AliHLTTPCCAGBHit ), nHits, file );
  assert( written == nHits );
  std::fflush( file );
  UNUSED_PARAM1(written);
}

void AliHLTTPCCAGBTracker::ReadEvent( FILE *file )
{
  //* Read event from file

  StartEvent();
  int nHits;
  int read = std::fread( &nHits, sizeof( int ), 1, file );
  assert( read == 1 );
  SetNHits( nHits );
  read = std::fread( fHits.Data(), sizeof( AliHLTTPCCAGBHit ), nHits, file );
  assert( read == nHits );
  UNUSED_PARAM1(read);
    // check
//   for (int iHit = 0; iHit <= fHits.Size(); iHit++){
//     std::cout << "iHit " << iHit << std::endl;
//     std::cout << fHits.Data()[iHit].X() << " "
//               << fHits.Data()[iHit].Y() << " "
//               << fHits.Data()[iHit].Z() << std::endl;
//     std::cout << fHits.Data()[iHit].ErrX() << " "
//               << fHits.Data()[iHit].ErrY() << " "
//               << fHits.Data()[iHit].ErrZ() << std::endl;
//   }
  
    // remove 13 row (iRow = 12) IKu
//   for (int iHit = 0; iHit <= fHits.Size(); iHit++){
//     if (fHits.Data()[iHit].IRow() >= 13)
//       fHits.Data()[iHit].SetIRow(fHits.Data()[iHit].IRow()-1);
//   }
}

void AliHLTTPCCAGBTracker::WriteTracks( std::ostream &out ) const
{
  //* Write tracks to file

  out << fSliceTrackerTime << std::endl;
  int nTrackHits = 0;
  for ( int itr = 0; itr < fNTracks; itr++ ) {
    nTrackHits += fTracks[itr].NHits();
  }
  out << nTrackHits << std::endl;
  for ( int ih = 0; ih < nTrackHits; ih++ ) {
    out << fTrackHits[ih] << " ";
  }
  out << std::endl;

  out << fNTracks << std::endl;
  for ( int itr = 0; itr < fNTracks; itr++ ) {
    AliHLTTPCCAGBTrack &t = fTracks[itr];
    const AliHLTTPCCATrackParam &p = t.Param();
    out << t.NHits() << " ";
    out << t.FirstHitRef() << " ";
    out << t.DeDx() << std::endl;
    out << p;
  }
}

void AliHLTTPCCAGBTracker::ReadTracks( std::istream &in )
{
  //* Read tracks  from file

  in >> fTime;
  fSliceTrackerTime = fTime;
  fStatTime[0] += fTime;
  fStatNEvents++;
  delete[] fTrackHits;
  fTrackHits = 0;
  int nTrackHits = 0;
  in >> nTrackHits;
  fTrackHits = new int [nTrackHits];
  for ( int ih = 0; ih < nTrackHits; ih++ ) {
    in >> TrackHits()[ih];
  }
  delete[] fTracks;
  fTracks = 0;
  in >> fNTracks;
  fTracks = new AliHLTTPCCAGBTrack[fNTracks];
  for ( int itr = 0; itr < fNTracks; itr++ ) {
    AliHLTTPCCAGBTrack &t = Tracks()[itr];
    in >> t;
  }
}

void AliHLTTPCCAGBTracker::SetHits( std::vector<AliHLTTPCCAGBHit> &hits)
{
  const int NHits2 = hits.size();

  SetNHits(NHits2);

  fHits.Resize(NHits2);
  for (int iH = 0; iH < NHits2; iH++){
    fHits[iH] = hits[iH];
  }
} // need for StRoot

void AliHLTTPCCAGBTracker::SaveHitsInFile(string prefix) const
{
    ofstream ofile((prefix+"hits.data").data(),std::ios::out|std::ios::app);
    const int Size = fHits.Size();
    ofile << Size << std::endl;
    for (int i = 0; i < fHits.Size(); i++){
      const AliHLTTPCCAGBHit &l = fHits[i];
      ofile << l;
    }
    ofile.close();

}

void AliHLTTPCCAGBTracker::SaveSettingsInFile(string prefix) const
{
  ofstream ofile((prefix+"settings.data").data(),std::ios::out|std::ios::app);
  WriteSettings(ofile);
}

bool AliHLTTPCCAGBTracker::ReadHitsFromFile(string prefix)
{
    ifstream ifile((prefix+"hits.data").data());
    if ( !ifile.is_open() ) return 0;
    int Size;
    ifile >> Size;
    fHits.Resize(Size);
    SetNHits(Size);
    fFStrips.Resize(Size); // create a virtual strip for each hit (currently strips position is not used for ITS, so don't have to fill them)
    fBStrips.Resize(Size); // create a virtual strip for each hit

    for (int i = 0; i < Size; i++){
      AliHLTTPCCAGBHit &l = fHits[i];
      ifile >> l;
      l.SetFStripP( &fFStrips[i] );
      l.SetBStripP( &fBStrips[i] );
    }
    ifile.close();
    return 1;
}

bool AliHLTTPCCAGBTracker::ReadSettingsFromFile(string prefix)
{
  ifstream ifile((prefix+"settings.data").data());
  if ( !ifile.is_open() ) return 0;
  ReadSettings(ifile);
  return 1;
}

void AliHLTTPCCAGBTracker::CATrackFinder()
{
  int curHit = 0; // counters, used to save tracks in output format
  int curTr = 0;


    // prepare memory for tracks
  if(fTracks) delete [] fTracks;
  fTracks = new AliHLTTPCCAGBTrack[2000]; // TODO
  fNTracks = 0;
  if(fTrackHits) delete [] fTrackHits;
  fTrackHits = new int[30000*AliHLTTPCCAParameters::MaxNStations]; // TODO

    // std::sort( fHits.Data(), fHits.Data() + fNHits, AliHLTTPCCAGBHit::Compare ); to make convertion faster
  ITSCAHits hits(NStations());
    // convert hits
  for( int iH = 0; iH < fNHits; ++iH ) {
    hits.Add( ITSCAHit( fHits[iH], iH ) );
  }

  hits.Sort();
  
    /// Find Tracks
  ITSCATarget target;
  for( fFindIter = 0; fFindIter < fNFindIterations; ++fFindIter ) {
    // fFindIter = 2; // dbg
    ITSCAHitsV hitsV(hits); // get SIMDized hits
  
#ifdef DRAW_CA
    AliHLTTPCCADisplay::Instance().SetTPCView();
    AliHLTTPCCADisplay::Instance().DrawTPC();
    AliHLTTPCCADisplay::Instance().ClearView();
    AliHLTTPCCADisplay::Instance().DrawGBHits(hitsV);
    AliHLTTPCCADisplay::Instance().Ask();
#endif
    
      // set parameters; TODO depend on iteration
    const float kCorr = 1.2; // correction on pulls width
    const float kCorr2 = kCorr*kCorr;
    fPick_m = 3.*kCorr;
    fPick_r = 5.*kCorr;
    fPickNeighbour = 3.*kCorr;
    TRACK_CHI2_CUT = 10.*kCorr2;
    TRIPLET_CHI2_CUT = 5.*kCorr2;
      // Set correction in order to take into account overlaping
      // The reason is that low momentum tracks are too curved and goes not from target direction. That's why hit sort is not work idealy
    fMaxDX0 = 0;
    switch ( fFindIter ) { // fast primary
      case 0:
        fMaxInvMom = 1.f; // > 1000 MeV tracks
        target = ITSCATarget( 0, 0, 0, 0.1, 0.1, fMaxInvMom/3.f, GetParameters().VtxFieldValue(), 2 ); // target at 0 with 0.1 cm error and 2 NDF (for debug)
        fFindGappedTracks = false;
        fMaxDX0 = 0;
        break;
      case 1:  // all primary
        fMaxInvMom = 20.f; // > 50 MeV tracks
        target = ITSCATarget( 0, 0, 0, 1, 10, fMaxInvMom/3.f, GetParameters().VtxFieldValue(), 1 );
        fMaxDX0 = 0.1;
        fFindGappedTracks = true;
        break;
      case 2:  // all
        fMaxInvMom = 10.f;
        target = ITSCATarget( 0, 0, 0, 10, 10, fMaxInvMom/3.f, GetParameters().VtxFieldValue(), 0 );
        fMaxDX0 = 0.1;
        fFindGappedTracks = false;
        break;
    }

      // reconstruct
    ITSCASingletsV singletsV( NStations(), &hits );
    CreateSinglets( target, hitsV, singletsV );

    fPick = fPick_m;
    ITSCADoubletsV doubletsV( NStations()-1, &hits );
    CreateDoublets( singletsV, doubletsV );
    
    fPick = fPick_r;
    ITSCATripletsV tripletsV( NStations()-2, &hits );
    CreateTriplets( doubletsV, tripletsV );

    ITSCATriplets triplets(tripletsV);

    fPick = fPickNeighbour;
    FindNeighbours( triplets );
  
    ITSCATracks tracks(&hits);
    CreateTracks( triplets, tracks );
        
      // save tracks in compact format
    const int NRTracks = tracks.size();

    for(int iT=0; iT<NRTracks; iT++)
    {
      const int NTHits = tracks[iT].NHits();

      fTracks[curTr].SetNHits( NTHits );
      fTracks[curTr].SetFirstHitRef( curHit );

      for(int iH=0; iH < NTHits; iH++)
      {
        fTrackHits[curHit] = tracks.Hit(iH, iT).Id();
        curHit++;
      }
      curTr++;
    }
    fNTracks += NRTracks;
    
    hits.Clean(); // remove used hits
  }
  
  // Merge( tracks );
}

void AliHLTTPCCAGBTracker::CreateSinglets( const ITSCATarget& target, const ITSCAHitsV& hits, ITSCASingletsV& singlets )
{
  for( char iS = 0; iS < NStations(); ++iS ) {
    singlets.OnStation( iS ) = Combine( target, hits.OnStation( iS ) );
  }
#ifdef DRAW_CA
  AliHLTTPCCADisplay::Instance().DrawGBSinglets(singlets);
  AliHLTTPCCADisplay::Instance().Ask();
#endif
}

void AliHLTTPCCAGBTracker::CreateDoublets( const ITSCASingletsV& singlets, ITSCADoubletsV& doublets )
{
  if ( fFindGappedTracks ) {
    for( int iS = 0; iS < NStations()-2; ++iS ) {
      doublets.OnStation( iS ) = Combine( singlets.OnStation( iS ), singlets.OnStation( iS + 1 ) ) + Combine( singlets.OnStation( iS ), singlets.OnStation( iS + 2 ) );
    }
    int iS = NStations()-2;
    doublets.OnStation( iS ) = Combine( singlets.OnStation( iS ), singlets.OnStation( iS + 1 ) );
  }
  else
    for( int iS = 0; iS < NStations()-1; ++iS ) {
      doublets.OnStation( iS ) = Combine( singlets.OnStation( iS ), singlets.OnStation( iS + 1 ) );
    }
#ifdef DRAW_CA
  AliHLTTPCCADisplay::Instance().DrawGBDoublets(doublets);
  AliHLTTPCCADisplay::Instance().Ask();
#endif
}

void AliHLTTPCCAGBTracker::CreateTriplets( const ITSCADoubletsV& doublets, ITSCATripletsV& triplets )
{
  if ( fFindGappedTracks ) {
    for( int iS = 0; iS < NStations()-3; ++iS ) {
      triplets.OnStation( iS ) = Combine( doublets.OnStation( iS ), doublets.OnStation( iS + 1 ) ) + Combine( doublets.OnStation( iS ), doublets.OnStation( iS + 2 ) );
    }
    int iS = NStations()-3;
    triplets.OnStation( iS ) = Combine( doublets.OnStation( iS ), doublets.OnStation( iS + 1 ) );
  }
  else
    for( int iS = 0; iS < NStations()-2; ++iS ) {
      triplets.OnStation( iS ) = Combine( doublets.OnStation( iS ), doublets.OnStation( iS + 1 ) );
    }
#ifdef DRAW_CA
  AliHLTTPCCADisplay::Instance().DrawGBTriplets(triplets);
  AliHLTTPCCADisplay::Instance().SaveCanvasToFile( "DrawTriplets.pdf" );
  AliHLTTPCCADisplay::Instance().Ask();
#endif
}

  // find and store neighbours triplets
  // to make recursive search faster saves only neighbours with level = level - 1
void AliHLTTPCCAGBTracker::FindNeighbours( ITSCATriplets& triplets )
{
  for( int iS = NStations()-4; iS >= 0; --iS ) {
    ITSCAElementsOnStation<ITSCATriplet>& ts1 = triplets.OnStation( iS );
    
    for( unsigned int iT1 = 0; iT1 < ts1.size(); ++iT1 ) {
      ITSCATriplet& t1 = ts1[iT1];

      if ( t1.ISta(1) >= triplets.NStations() ) continue;
      ITSCAElementsOnStation<ITSCATriplet>& ts2 = triplets.OnStation( t1.ISta(1) );
      
      char maxLevel = -1; // maxLevel of neighbour triplets
      vector<unsigned int> neighCands; // save neighbour candidates
      for( unsigned int iT2 = 0; iT2 < ts2.size(); ++iT2 ) {
        const ITSCATriplet& t2 = ts2[iT2];
        if( !t1.IsRightNeighbour( fPick, t2 ) ) continue;

        if ( maxLevel <  t2.Level() ) maxLevel = t2.Level();
        if ( maxLevel == t2.Level() ) neighCands.push_back(iT2);
      }
      t1.Level() = maxLevel + 1;

        // save
      for( unsigned int iN = 0; iN < neighCands.size(); ++iN ) {
        const ITSCATriplet& t2 = ts2[ neighCands[iN] ];

        if ( maxLevel == t2.Level() ) t1.INeighbours().push_back( neighCands[iN] );
      }
    } // iTrip1
  } // iStation
}

void AliHLTTPCCAGBTracker::CreateTracks( const ITSCATriplets& triplets, ITSCATracks& tracks )
{
  int min_level = AliHLTTPCCAParameters::MinimumHitsForRecoTrack - 3; // min level to start triplet. So min track length = min_level+3.
    // collect consequtive: the longest tracks, shorter, more shorter and so on
  for (int ilev = NStations()-3; ilev >= min_level; ilev--){ // choose length
    ITSCATracks vtrackcandidate( tracks.HitsRef() );
      
      //  how many levels to check
    int nlevel = (NStations()-2)-ilev+1;
    const unsigned char min_best_l = (ilev > min_level) ? ilev + 2 : min_level + 3; // lose maximum one hit and find all hits for min_level
      // const unsigned char min_best_l = ilev + 3; // find all hits (slower)

      // find candidates
    for( int istaF = 0; istaF <= NStations()-3-ilev; istaF++ ){
      const ITSCAElementsOnStation<ITSCATriplet>& tsF = triplets.OnStation( istaF );

      for( unsigned int iTrip=0; iTrip < tsF.size(); iTrip++ ){
        const ITSCATriplet* tripF = &(tsF[iTrip]);

        if (0) { // ghost supression !!!
          if ( tripF->Level() == 0 ) continue; // ghost suppression // find track with 3 hits only if it was created from a chain of triplets, but not from only one triplet
          if ( tripF->Level() < ilev ) continue; // try only triplets, which can start track with ilev+3 length. w\o it have more ghosts, but efficiency either
          if ( (ilev == 0) && (tripF->ISta(0) != 0) ) continue;  // ghost supression // collect only MAPS tracks-triplets
        }

        if( tsF.GetHit( 0, iTrip ).IsUsed() ) continue;
        
        ITSCATrack curr_tr;
        curr_tr.AddHit( tripF->IHit(0) );
        curr_tr.Chi2() = tripF->Param().Chi2();
        
        ITSCATrack best_tr = curr_tr;

        FindBestCandidate(istaF, best_tr, iTrip, curr_tr, min_best_l, triplets);

        if ( best_tr.NHits() < min_best_l ) continue;
        if ( best_tr.NHits() < AliHLTTPCCAParameters::MinimumHitsForRecoTrack ) continue;
          
        int ndf = best_tr.NHits()*2 - 5;
        best_tr.Chi2() /= ndf;

        vtrackcandidate.push_back(best_tr);
      }
    } // istaF

      // select and save best candidates
    if (--nlevel == 0) break;
    vtrackcandidate.SelectAndSaveTracks( tracks );
  } // ilev

#ifdef DRAW_CA
    AliHLTTPCCADisplay::Instance().DrawGBTracks(tracks);
    AliHLTTPCCADisplay::Instance().Ask();
#endif
}

void AliHLTTPCCAGBTracker::Merge( ITSCATracks& tracks )
{
  UNUSED_PARAM1( tracks ); // TODO
  // currently is not needed
}

ITSCAElementsOnStation<ITSCASingletV> AliHLTTPCCAGBTracker::Combine( const ITSCATarget& t, const ITSCAElementsOnStation<ITSCAHitV>& h)
{
  ITSCAElementsOnStation<ITSCASingletV> r;

  for( unsigned short iH = 0; iH < h.size(); ++iH ) {
    const ITSCAHitV& hit = h[iH];
    AliHLTTPCCATrackParamVector param;
    param.InitByHit( hit, GetParameters(), t.DQMom() );
    param.InitDirection( hit.X0() - t.X0(), hit.X1() - t.X1(), hit.X2() - t.X2() ); // works only of t.X() = Y() = 0
    param.SetAngle( hit.Angle() );
    
    sfloat_m active = hit.IsValid();
    active &= param.AddTarget( t, active );
    
    r.push_back( ITSCASingletV( ushort_v::IndexesFromZero() + iH*ushort_v::Size, h.IStation(), param, active ) );
  }

  return r;
}

ITSCAElementsOnStation<ITSCADoubletV> AliHLTTPCCAGBTracker::Combine( const ITSCAElementsOnStation<ITSCASingletV>& a, const ITSCAElementsOnStation<ITSCASingletV>& b)
{
  ITSCAElementsOnStation<ITSCADoubletV> r;

  unsigned int iStartS2 = 0; // save first good singet2 for last singlet1
  for( unsigned int iS1 = 0; iS1 < a.size(); ++iS1 ) {
    const ITSCASingletV& s1 = a[iS1];
    const sfloat_m valid1 = s1.IsValid();

      // find iStartS2
    bool firstFound = false;
    unsigned int iStartS2V = iStartS2/sfloat_v::Size;
    unsigned int iStartS2E = iStartS2%sfloat_v::Size;
    for( unsigned int iS2 = iStartS2V; iS2 < b.size() && !firstFound; ++iS2 ) {
      const ITSCASingletV& s2 = b[iS2];
      const sfloat_m valid2 = s2.IsValid() && static_cast<sfloat_m>(ushort_v(Vc::IndexesFromZero) >= iStartS2E);
      foreach_bit( unsigned int iV, valid2 ) { // try each hit from 2nd singlet with all 1st singlets
        if(firstFound) continue;
        const ITSCAHit& hit = b.GetHit(iV, 0, iS2);
 
        sfloat_m active = valid1;
        AliHLTTPCCATrackParamVector param = s1.Param();

        active &= param.Transport( hit, GetParameters(), active );
        
        const sfloat_v Pick2 = fPick*fPick;
        const sfloat_v dx2 = hit.X2() - param.X2() + 2*fMaxDX0*abs(param.Tx2());
        const sfloat_v dx2_est2 = Pick2*(abs(param.Err2X2() + hit.Err2X2()));
        const sfloat_m inRange = (dx2*dx2 <= dx2_est2) || (dx2 > 0);

        if ( (!active || (active && !inRange)).isFull() ) continue;
  
        iStartS2 = iS2*sfloat_v::Size + iV;
        firstFound = true;
        // break; // doesn't work
      } // iV
      iStartS2E = 0;
    } // iS2
    
    iStartS2V = iStartS2/sfloat_v::Size;
    iStartS2E = iStartS2%sfloat_v::Size;
    for( unsigned int iS2 = iStartS2V; iS2 < b.size(); ++iS2 ) {
      const ITSCASingletV& s2 = b[iS2];
      const sfloat_m valid2 = s2.IsValid() && static_cast<sfloat_m>(ushort_v(Vc::IndexesFromZero) >= iStartS2E);
      foreach_bit( unsigned int iV, valid2 ) { // try each hit from 2nd singlet with all 1st singlets
        const ITSCAHit& hit = b.GetHit(iV, 0, iS2);
 
        sfloat_m active = valid1;
        AliHLTTPCCATrackParamVector param = s1.Param();

        active &= param.Transport( hit, GetParameters(), active );

        const sfloat_v Pick2 = fPick*fPick;
        const sfloat_v dx2 = hit.X2() - param.X2() - 2*fMaxDX0*abs(param.Tx2());
        const sfloat_v dx2_est2 = Pick2*(abs(param.Err2X2() + hit.Err2X2()));
        const sfloat_m inRange = ( (dx2*dx2 <= dx2_est2) || (dx2 < 0) );
        if ( (!valid1 || (active && !inRange)).isFull() ) break;

        active &= IsEqual( param, hit );
        if ( active.isEmpty() ) continue;
        
        active &= param.Filter( hit, GetParameters(), active );
        
        active &= param.Chi2() < TRIPLET_CHI2_CUT * static_cast<sfloat_v>(param.NDF() + 2);
        if ( active.isEmpty() ) continue;

            // param.Extrapolate( nextSta );

        r.push_back( ITSCADoubletV( s1, s2, iV, param, active ) );
      } // iV
      iStartS2E = 0;
    } // iS2
  } // iS1
  
  return r;
}


ITSCAElementsOnStation<ITSCATripletV> AliHLTTPCCAGBTracker::Combine( const ITSCAElementsOnStation<ITSCADoubletV>& a, const ITSCAElementsOnStation<ITSCADoubletV>& b)
{
  ITSCAElementsOnStation<ITSCATripletV> r;
    
  for( unsigned int iD1 = 0; iD1 < a.size(); ++iD1 ) {
    const ITSCADoubletV& D1 = a[iD1];
    const sfloat_m valid1 = D1.IsValid();
    for( unsigned int iD2 = 0; iD2 < b.size(); ++iD2 ) { // try each 2nd hit from 2nd doubletVector with all 1st doublets
      const ITSCADoubletV& D2 = b[iD2];
      const sfloat_m valid2 = D2.IsValid();

      foreach_bit( unsigned int iV, valid2 ) {
        sfloat_m active = valid1;
        
        active &= D1.IHit(1) == D2.IHit(0)[iV];
        if ( active.isEmpty() ) continue;
      
        const ITSCAHit& hit = b.GetHit(iV, 1, iD2 );
      
        AliHLTTPCCATrackParamVector param = D1.Param();

        active &= param.Transport( hit, GetParameters(), active );

        active &= IsEqual( param, hit );
        if ( active.isEmpty() ) continue;
      
        active &= param.Filter( hit, GetParameters(), active );

        active &= param.Chi2() < TRIPLET_CHI2_CUT * static_cast<sfloat_v>(param.NDF());
        if ( active.isEmpty() ) continue;
      
        r.push_back( ITSCATripletV( D1, D2, iV, param, active ) );
      }
    }
  }
  
  return r;
}

void AliHLTTPCCAGBTracker::FindBestCandidate(int ista,
                         ITSCATrack &bT, // best track
                         int currITrip,  // index of current triplet
                         ITSCATrack &cT, // current track
                         unsigned char min_best_l,
                         const ITSCATriplets& triplets)
{
  const ITSCAElementsOnStation<ITSCATriplet>& trs = triplets.OnStation( ista );
  const ITSCATriplet* curr_trip = &(trs[currITrip]);
  
  if (curr_trip->Level() == 0){ // the end of the track -> check and store
    
      // -- finish with current track
      // add rest of hits
    if( !trs.GetHit( 1, currITrip ).IsUsed() ) {
      cT.AddHit( curr_trip->IHit(1) );

      if( !trs.GetHit( 2, currITrip ).IsUsed() )
        cT.AddHit( curr_trip->IHit(2) );
    }
    
    if( cT.NHits() < min_best_l - 1 ) return; // suppose that only one hit can be added by extender
    if( cT.Chi2() > TRACK_CHI2_CUT * (cT.NHits()*2-5) ) return;

      // -- select the best
    if ( (cT.NHits() > bT.NHits()) || ( (cT.NHits() == bT.NHits()) && (cT.Chi2() < bT.Chi2()) ) )
      bT = cT;

  } else
  { // level != 0

      // try to extend. try all possible triplets
    int NNeighs = curr_trip->INeighbours().size();
    for (int in = 0; in < NNeighs; in++) {
      int newITrip = curr_trip->INeighbours()[in];
      const ITSCATriplet *new_trip = &(triplets.OnStation( curr_trip->ISta(1) )[newITrip]);

        // check new triplet
      const float qp1 = curr_trip->QMomentum();
      const float qp2 = new_trip->QMomentum();
      float dqp = fabs(qp1 - qp2);
      float Cqp = curr_trip->QMomentumErr();
      Cqp      += new_trip->QMomentumErr();

      if ( triplets.OnStation( curr_trip->ISta(1) ).GetHit( 1, newITrip ).IsUsed() ){ 
          //  no used hits allowed -> compare and store track
        if ( (cT.NHits() > bT.NHits()) || ( (cT.NHits() == bT.NHits()) && (cT.Chi2() < bT.Chi2()) ) )
          bT = cT;
      }
      else{ //  add new triplet to the current track

          // restore current track
          // save current tracklet
        ITSCATrack nT = cT; // new track

          // add new hit
        nT.AddHit( new_trip->IHit(0) );
        dqp = dqp/Cqp*5.;
        nT.Chi2() += dqp*dqp;
        
        if( nT.Chi2() > TRACK_CHI2_CUT * nT.NHits() ) continue;

        FindBestCandidate(new_trip->ISta(0), bT, newITrip, nT, min_best_l, triplets);
      } // add triplet to track
    } // for neighbours
  } // level = 0
}

sfloat_m AliHLTTPCCAGBTracker::IsEqual(
  const AliHLTTPCCATrackParamVector& param, const ITSCAHit& hit )
{
  sfloat_m r = sfloat_m(true);
  
  const sfloat_v Pick2 = fPick*fPick;
  const sfloat_v dx1 = hit.X1() - param.X1();
  const sfloat_v dx1_est2 = Pick2*(abs(param.Err2X1() + hit.Err2X1()));
  r &= dx1*dx1 < dx1_est2;

  const sfloat_v dx2 = hit.X2() - param.X2();
  const sfloat_v dx2_est2 = Pick2*(abs(param.Err2X2() + hit.Err2X2()));
  r &= dx2*dx2 < dx2_est2;
  
  return r;
}