// $Id: AliHLTTPCCAGBTracker.cxx,v 1.12 2010/09/01 10:38:27 ikulakov Exp $
// **************************************************************************
// This file is property of and copyright by the FIAS PANDA group           *
// PANDA Experiment at FIAS-GSI, All rights reserved.                       *
//                                                                          *
// Primary Authors: Ivan Kisel <I.Kisel@compeng.uni-frankfurt.de>           *
//                  Igor Kulakov <I.Kulakov@gsi.de>                         *
//                  Maksym Zyzak <M.Zyzak@gsi.de>                           *
//                  Valentina Akishina <V.Akishina@gsi.de>                  *
//		    Elizaveta Iakovleva <E.Iakovleva@gsi.de>                *
//                  for The FIAS PANDA group .                              *
//                                                                          *
// 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.                    *
//                                                                          *
//***************************************************************************


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

#ifdef STAR_HFT
  #define START_FROM_TARGET // how to fit
#elif defined(PANDA_STT) || defined(PANDA_FTS)
  #define START_FROM_TARGET
#else
#endif

#include "AliHLTTPCCAGBTracker.h"
#include "AliHLTTPCCAGBHit.h"
#include "AliHLTTPCCAGBTrack.h"
// #include "AliHLTTPCCATrackParam.h"
#include "AliHLTArray.h"
#include "AliHLTTPCCAMath.h"
#if !defined(PANDA_FTS)
#include "AliHLTTPCCATrackLinearisationVector.h"
#endif
#include "AliHLTTPCCAPerformance.h"
#include "TStopwatch.h"
#include "L1Timer.h"

#include "ITSCATarget.h"
#include "ITSCAHits.h"
#include "ITSCAHitsV.h"
#include "ITSCATracks.h"

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

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


bool SINGLE_THREADED = false;

AliHLTTPCCAGBTracker::AliHLTTPCCAGBTracker()
    :
    fHits( 0 ),
    fNHits( 0 ),
    fTrackHits( 0 ),
    fTracks( 0 ),
    fNTracks( 0 ),
    fTime( 0 ),
    fStatNEvents( 0 ),
    fSliceTrackerTime( 0 ),
    fSliceTrackerCpuTime( 0 ),
    fGTi(),
    fTi(fNFindIterations),
    fStatGTi(),
    fStatTi(fNFindIterations)
{
    //*  constructor

  for ( int i = 0; i < 20; i++ ) fStatTime[i] = 0;


  fGTi.Add("init  ");
  fGTi.Add("iters ");
  fGTi.Add("tracker");
  fGTi.Add("fitter ");
  
  fTi.SetNIter( fNFindIterations ); // for iterations
  fTi.Add("init  ");
  
  fTi.Add("1plet ");
  fTi.Add("2plet ");
  fTi.Add("3plet ");
  fTi.Add("4plet ");
  fTi.Add("5plet ");
  fTi.Add("6plet ");
  fTi.Add("convrt");
  
  fTi.Add("plets ");

  fTi.Add("nghbrs"); 
  fTi.Add("tracks");
  fTi.Add("merger");
  fTi.Add("finish");

  fStatGTi = fGTi;
  fStatTi  = fTi;
}

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()
{ 
  //* Creation of ideal tracks based on hits, which correspond to the MC-points.
  
  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];
    for( int k = 1; k < 3 && trackId < 0; k++ )
      trackId = perf->HitLabel(id).fLab[k];
    if ( trackId < 0 ) continue;
    
    hits[trackId].push_back(iH);
  }

  vector< vector<bool> > isRowUsed(NMCTracks);
  for(int iTr=0; iTr<NMCTracks; iTr++)
    isRowUsed[iTr].resize(AliHLTTPCCAParameters::MaxNStations,0);

#ifdef DRIFT_TUBES
  int nTracks = 0;
  for(int iT=0; iT<NMCTracks; iT++)// nTracks++; 
    if(hits[iT].size()>0)
    {
      int nFirstMC = (*perf->GetMCTracks())[iT].FirstMCPointID();
      int nMCPoints = (*perf->GetMCTracks())[iT].NMCPoints();
      AliHLTTPCCALocalMCPoint *points = &((*perf->GetMCPoints()).Data()[nFirstMC]);
      vector<int> hits_new;
      double xCur=0, yCur=0;
      for(int iP=0; iP<nMCPoints; iP++)
      {
        //if ( (points[iP].IRow()/2)%4 == 1 || (points[iP].IRow()/2)%4 == 2 ) continue; // skip skewed rows in panda_fts
        int curHit=-1;
        double minD = 2;//10e10; tube is 0.5 cm in radius, 2 cm should be enough
        for(unsigned int iH=0; iH<hits[iT].size(); iH++)
        {
          const AliHLTTPCCAGBHit &h = fHits[hits[iT][iH]];
          if ( points[iP].IRow() != h.IRow() ) continue;
          
          const double d = h.DistanceFromWireToPoint( points[iP].X(), points[iP].Y(), points[iP].Z() );
          if( d < minD )
          {
            minD=d;
            curHit = iH;
          }
        }

        const AliHLTTPCCAGBHit &hit = fHits[hits[iT][curHit]];
        if(iP==0)
        {
            xCur = hit.X();
            yCur = hit.Y();
        }
#ifdef PANDA_STT
        double dX = xCur - hit.X();
        double dY = yCur - hit.Y();

        double dr = CAMath::Sqrt(dX*dX + dY*dY);
        if(curHit > -1 && dr < 10) {
#else
        if(curHit > -1) {
#endif
          const int rowHit = hit.IRow();
          if( !isRowUsed[iT][rowHit] )
          {
            isRowUsed[iT][rowHit] = 1;
            xCur = hit.X();
            yCur = hit.Y();
            hits_new.push_back(hits[iT][curHit]);
          }
        }
      }
      hits[iT] = hits_new;
      if(hits[iT].size() >= 6)

      nTracks++;
    }
#else // DRIFT_TUBES
  int nTracks = NMCTracks;
#endif // DRIFT_TUBES
  
  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++)
  {
#ifdef DRIFT_TUBES
    if(hits[iT].size() <6) continue;
#else
    if(hits[iT].size() <4) continue;
#endif

    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++;
  }

}

void AliHLTTPCCAGBTracker::FitTracks()
{
  //* This function calls the function FitTrack for 
  //* fitting tracks and displays information on the screen.
  
  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;
    nTrackHits = ushort_v(Vc::Zero); 
    memFirstHits = ushort_v(Vc::Zero);

    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().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]];
#ifdef DRAW_FIT_LOCAL
        {
          float xLoc, yLoc, zLoc;
          AliHLTTPCCAParameters::GlobalToCALocal( h.X(), h.Y(), h.Z(), h.Angle(), xLoc, yLoc, zLoc );
          AliHLTTPCCADisplay::Instance().DrawGBPoint( xLoc, yLoc, zLoc, kGreen+1);
        }

        const int iMCP = perf->GetMCPoint( h );
        if (iMCP < 0) continue;
        const AliHLTTPCCALocalMCPoint &mcPoint = (*perf->GetMCPoints())[iMCP];
        {
          float xLoc, yLoc, zLoc;
          AliHLTTPCCAParameters::GlobalToCALocal( mcPoint.X(), mcPoint.Y(), mcPoint.Z(), h.Angle(), xLoc, yLoc, zLoc );
          AliHLTTPCCADisplay::Instance().DrawGBPoint( xLoc, yLoc, zLoc, kRed, (Size_t)1);
        }
#else
        AliHLTTPCCADisplay::Instance().DrawGBPoint(h.X(), h.Y(), h.Z(), h.Angle(), kGreen+1);
#endif
      }
    }

    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];
      for( int iP = 0; iP < mc.NMCPoints(); iP++ ) {
        AliHLTTPCCALocalMCPoint mcPoint =  (*perf->GetMCPoints())[mc.FirstMCPointID()+iP];
#ifdef DRAW_FIT_LOCAL
        // float xLoc, yLoc, zLoc;
        // AliHLTTPCCAParameters::GlobalToCALocal( mcPoint.X(), mcPoint.Y(), mcPoint.Z(), mcPoint.Angle(), xLoc, yLoc, zLoc );
        // AliHLTTPCCADisplay::Instance().DrawGBPoint( xLoc, yLoc, zLoc, kRed, (Size_t)1);
#else
        double mcX0 =  mcPoint.X();
        double mcY0 =  mcPoint.Y();
        double mcZ  =  mcPoint.Z();
        AliHLTTPCCADisplay::Instance().DrawGBPoint((float)mcX0, (float)mcY0, (float)mcZ, kRed, (Size_t)1);
#endif
      }
    }
    AliHLTTPCCADisplay::Instance().Ask();
#endif

    AliHLTTPCCATrackParamVector vStartPoint;
    AliHLTTPCCATrackParamVector vEndPoint;
    vStartPoint.ConvertTrackParamToVector(startPoint, 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
#ifdef DRAW_FIT
      // AliHLTTPCCADisplay::Instance().ClearView();
      // AliHLTTPCCADisplay::Instance().SetTPCView();
      // AliHLTTPCCADisplay::Instance().DrawTPC();
      
      AliHLTTPCCADisplay::Instance().DrawGBPoint(0,0,0, kMagenta, 0.5); // target

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

    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]];
#ifdef DRAW_FIT_LOCAL
          float xLoc, yLoc, zLoc;
          AliHLTTPCCAParameters::GlobalToCALocal( h.X(), h.Y(), h.Z(), h.Angle(), xLoc, yLoc, zLoc );
          AliHLTTPCCADisplay::Instance().DrawGBPoint( xLoc, yLoc, zLoc, iV);
#else
          AliHLTTPCCADisplay::Instance().DrawGBPoint(h.X(), h.Y(), h.Z(), h.Angle(),iV);
#endif
      }
      }
      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]];
#ifdef DRAW_FIT_LOCAL
          float xLoc, yLoc, zLoc;
          AliHLTTPCCAParameters::GlobalToCALocal( h.X(), h.Y(), h.Z(), h.Angle(), xLoc, yLoc, zLoc );
          AliHLTTPCCADisplay::Instance().DrawGBPoint( xLoc, yLoc, zLoc, iV);
#else
          AliHLTTPCCADisplay::Instance().DrawGBPoint(h.X(), h.Y(), h.Z(), h.Angle(),iV);
#endif
        }
      }
      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 )
{
  //*Initial approximation for the track reconstruction, relized by the least-square technique.

    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);
  sfloat_v zTmp(yH0);
  const sfloat_v angleV0(angle0);
  sfloat_v xV0, yV0, zV0(zH0);
  AliHLTTPCCAParameters::GlobalToCALocal(xTmp, yTmp, zV0, angleV0, xV0, yV0, zV0);

  vector<sfloat_v> xV(nHitsMax), yV(nHitsMax), zV(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, 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]];

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

    xTmp.load(xH);
    yTmp.load(yH);
    zTmp.load(zH);
    
    AliHLTTPCCAParameters::GlobalToCALocal(xTmp, yTmp, zTmp, angleV0, xV[ihit], yV[ihit], zV[ihit]);
  }

  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;

    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 = 1.f - ey1 * ey1;
    ex1(ex1<sfloat_v(Vc::Zero)) = sfloat_v(Vc::Zero);
    ex1 = CAMath::Sqrt( ex1 );
    ex1( ex < Vc::Zero ) = -ex1;

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

    sfloat_v cci = 1.f / cc;
    cci(CAMath::Abs(cc) < 1.e-8f) = 1.e8f;
    sfloat_v exi = 1.f / ex;
    exi(CAMath::Abs(ex) < 1.e-8f) = 1.e8f;
    sfloat_v ex1i = 1.f / ex1;
    ex1i(CAMath::Abs(ex1) < 1.e-8f) = 1.e8f;
    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.0e-4f )  ? ( 2.f * CAMath::ASin( dSin ) / kappa ) : dl;

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

  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();
#ifdef DRAW_FIT_LOCAL
  assert(0); // TODO
#endif
  for(unsigned short 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.Angle(),iV);
    }
  }

  double xCL = (X+xV0)[0];
  double yCL = (Y+yV0)[0];
  double zCL = 0, xC=0, yC=0, zC=0;
  AliHLTTPCCAParameters::CALocalToGlobal(xCL,yCL,zCL, double(angleV0[0]), xC, yC, zC );
  double 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

#ifdef PANDA_FTS
  // TODO!
  UNUSED_PARAM1(track);
#else
  track.SetAngle(angleV0);
  track.SetSinPhi(-sinPhi0); // -1 factor is a correction for direction (should be from inner to outer, but previous formula calculate for fromOuterToInner)
  track.SetSignCosPhi(-signCosPhi0);
  track.SetX(xV0);
  track.SetY(yV0);
  track.SetZ(Z0);
  track.SetDzDs(-dzds);
  track.SetQPt(-kappa/GetParameters().cBz());
#endif
}

sfloat_m AliHLTTPCCAGBTracker::FitTrack( AliHLTTPCCATrackParamVector &t,
                                         ushort_v &firstHits,
                                         ushort_v::Memory &NTrackHits,
                                         int &nTracksV, sfloat_m active0, bool dir )
{
  //* Fitting of the track by using Kalman Filter, 
  //* taking into account changes of his parameters due crossing through the detector material.
  
#ifdef PANDA_FTS
  UNUSED_PARAM6( t, firstHits, NTrackHits, nTracksV, active0, dir );
  return sfloat_m(true);
#else
  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, zH, hitAlpha;
    sfloat_v::Memory memXOverX0, memXTimesRho;

    sfloat_v::Memory err2X1h, err2x2h, errX12h;
    short_v::Memory mHitNDF;
#ifdef DRIFT_TUBES
    sfloat_v::Memory rh, isLeftH;
#endif

    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();

      err2X1h[iV] = h.Err2X1();
#ifdef PANDA_FTS // TODO why "-" ?
      errX12h[iV] = -h.ErrX12();
#else
      errX12h[iV] = h.ErrX12();
#endif
      err2x2h[iV] = h.Err2X2();
#ifdef DRIFT_TUBES
      rh[iV] = h.R();
      isLeftH[iV] = h.IsLeft() ? 1.f : 0.f;
#endif
      mHitNDF[iV] = GetParameters().Station( h.IRow() ).NDF;
      memXOverX0[iV] = GetParameters().GetXOverX0(h.IRow());
      memXTimesRho[iV] = GetParameters().GetXTimesRho(h.IRow());
    }

    const sfloat_v err2X1(err2X1h), err2x2(err2x2h), errX12(errX12h);
#ifdef DRIFT_TUBES
    const sfloat_v isLeftF(isLeftH),r(rh);
    const sfloat_m isLeft = isLeftF > sfloat_v(Vc::Zero);
#endif
    const short_v hitNDF(mHitNDF);

    sfloat_v xV(xH);
    sfloat_v yV(yH);
    sfloat_v zV(zH);
    const sfloat_v hitAlphaV(hitAlpha);
    const sfloat_v xOverX0(memXOverX0);
    const sfloat_v xTimesRho(memXTimesRho);

#ifdef DRAW_FIT
#ifdef DRAW_FIT_LOCAL
  assert(0); // TODO
#endif
    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 x0 = xV, y0 = yV;
    AliHLTTPCCAParameters::GlobalToCALocal(x0, y0, zV, hitAlphaV, xV, yV, zV);

    t.SetAngle(hitAlphaV, active);
    const sfloat_m &transported = t.TransportToX0WithMaterial( xV, l, fitPar, xOverX0, xTimesRho, GetParameters().cBz(), 0.999f, active);

#ifdef DRAW_FIT
#ifdef DRAW_FIT_LOCAL
  assert(0); // TODO
#endif
    sfloat_v xL = t.X(), yL = t.Y(), zL = t.Z(), xGl, yGl, zGl;
    AliHLTTPCCAParameters::CALocalToGlobal(xL, yL, zL, t.Angle(), xGl, yGl, zGl);

    for(int iV=0; iV<nTracksV; iV++)
    {
      if(!active[iV]) continue;
      AliHLTTPCCADisplay::Instance().DrawGBPoint(xGl[iV], yGl[iV], zGl[iV],2,1.);
    }
    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( short_v(-5), static_cast<short_m>(active) );
      t.CalculateFitParameters( fitPar );
      t.SetAngle(sfloat_v( hitAlpha ));
      first = 0;
    }

    sfloat_v x1 = yV;
#ifdef DRIFT_TUBES
    sfloat_v sinPhi = t.SinPhi();
    sfloat_v xCorr = r - r/(sqrt(1-sinPhi*sinPhi));
    x1(isLeft) += xCorr;
    x1(!isLeft) -= xCorr;
#endif
    const sfloat_m &filtered = t.FilterWithMaterial(x1, zV, err2X1, errX12, err2x2, 0.999f, active, hitNDF);

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

#ifdef DRAW_FIT
#ifdef DRAW_FIT_LOCAL
  assert(0); // TODO
#endif
    xL = t.X(); yL = t.Y(); zL = t.Z();
    AliHLTTPCCAParameters::CALocalToGlobal(xL, yL, zL, t.Angle(), xGl, yGl, zGl);

    for(int iV=0; iV<nTracksV; iV++)
    {
      if(!active[iV]) continue;
      AliHLTTPCCADisplay::Instance().DrawGBPoint(xGl[iV], yGl[iV], zGl[iV],3,1.);
    }
    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.f, ok && static_cast<sfloat_m>(l.CosPhi() >= Vc::Zero) );
  t.SetSignCosPhi(-1.f, ok && static_cast<sfloat_m>(l.CosPhi() < Vc::Zero) );
  
  return ok;
#endif
}

  // 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();
  unsigned short FirstHit = 0; // check part of the track begining from x-th hit
  const bool AddLastHit = true;
#else // check tracklets
//  if (NTHits.max() < 7) return static_cast<sfloat_m>(false);
  const unsigned short NHitsMax = 6; // check x-lets fit
  const unsigned short FirstHit = 15; // check part of the track begining from x-th hit
  const bool AddLastHit = true;
#endif

  //active &= NTHits >= NHitsMax+FirstHit;
  if (active.isEmpty()) return active;
    
  vector<ITSCAHitV> hits( NHitsMax );

  for ( unsigned short ihit = FirstHit; (ihit-FirstHit) < NHitsMax; 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) {
      if ( dir )
        id[iV] = firstHits[iV] + (NTHits[iV]-static_cast<unsigned short>(1)) - ihit;
      else
        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 (defined(USE_MC_PV) && defined(STAR_HFT))
  const double *pv = AliHLTTPCCAPerformance::Instance().PV(); // dbg
  float xT = pv[0], yT = pv[1], zT = pv[2];
#else
  float xT = 0, yT = 0, zT = 0;
#endif
  
  fMaxInvMom = 20.f; // 0.1 GeV tracks
#if defined(PANDA_STT)
  ITSCATarget target( xT, yT, zT, 10, 10, fMaxInvMom/3.f, GetParameters(), 0 );
#elif  defined(PANDA_FTS)
  ITSCATarget target( xT, yT, zT, 10, 10, fMaxInvMom/3.f, GetParameters(), 0 );
#else
  ITSCATarget target( xT, yT, zT, 0.1, 10, fMaxInvMom/3.f, GetParameters(), 1 );
#endif
  
#ifdef START_FROM_TARGET  // target + hit
  // fMaxInvMom = 1.f; // 0.1 GeV tracks
  // ITSCATarget target( xT, yT, zT, 0.03, 0.03, fMaxInvMom/3.f, GetParameters(), 2 ); // target at 0 with 3 cm error // 0.1 GeV tracks
//  ITSCATarget target = fTarget;
  //target.SetErrQMom(20/3.f);
  
  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
#if (defined(USE_MC_PV) && defined(STAR_HFT))
  const double *pv = AliHLTTPCCAPerformance::Instance().PV(); // dbg
  float xT = pv[0], yT = pv[1], zT = pv[2];
#else
  float xT = 0, yT = 0, zT = 0;
#endif
    
  const ITSCAHitV& hit1 = hits[1];
  fMaxInvMom = 1.f; // 0.05 GeV tracks
  ITSCATarget target( xT, yT, zT, 10e10, 10e10, fMaxInvMom/3.f, GetParameters(), 0 ); // target at 0 with 3 cm error

  t.InitByTarget( target );
  t.SetAngle( hit0.Angle() );

  sfloat_v xg0,yg0,zg0,xg1,yg1,zg1;
  AliHLTTPCCAParameters::CALocalToGlobal( hit0.X0(), hit0.X1(), hit0.X2(), hit0.Angle(), xg0, yg0, zg0 );
  AliHLTTPCCAParameters::CALocalToGlobal( hit1.X0(), hit1.X1(), hit1.X2(), hit1.Angle(), xg1, yg1, zg1 );
  sfloat_v dx = xg1-xg0;
  sfloat_v dy = yg1-yg0;
  sfloat_v dz = zg1-zg0;
  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.Err2X1() );
  t.SetErr2Z( hit0.Err2X2() );

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

  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 // START_FROM_TARGET
    const ITSCAHitV& hit = hits[ihit];
    
    const sfloat_m transToHit = static_cast<sfloat_m>(ihit <= (NTHits - FirstHit - 1)); 
    active &= t.Transport( hit, GetParameters(), active ) || !transToHit;
#ifdef DRAW_FIT
    {
    sfloat_v xGl, yGl, zGl;
    AliHLTTPCCAParameters::CALocalToGlobal(t.X(), t.Y(), t.Z(), t.Angle(), xGl, yGl, zGl);
    foreach_bit( unsigned short iV, active ) {

#ifdef DRAW_FIT_LOCAL
    AliHLTTPCCADisplay::Instance().DrawGBPoint( t.X()[iV], t.Y()[iV], t.Z()[iV], kBlue+2, 0.5 );
    cout << t.X()[iV] << " " << t.Y()[iV] << " " << t.Z()[iV] << endl;
#else
    AliHLTTPCCADisplay::Instance().DrawGBPoint(xGl[iV], yGl[iV], zGl[iV], kBlue+2, 0.5);
#endif
  }
    AliHLTTPCCADisplay::Instance().Ask();
  }
#endif
    
    const sfloat_m addHit = static_cast<sfloat_m>((AddLastHit & ihit <= (NTHits - FirstHit - 1)) || (!AddLastHit & ihit < (NTHits - FirstHit - 1)));  // don't add last hit, thereby checking extrapolation
    active &= t.Filter( hit, GetParameters(), active & addHit ) || !addHit;
    
#ifdef DRAW_FIT
    {
      sfloat_v xGl, yGl, zGl;
      AliHLTTPCCAParameters::CALocalToGlobal(t.X(), t.Y(), t.Z(), t.Angle(), xGl, yGl, zGl);
      foreach_bit( unsigned short iV, active & hit.IsValid() ) {
        float gx,gy,gz;
        hit.GetGlobalCoor( iV, gx, gy, gz );
#ifdef DRAW_FIT_LOCAL
        AliHLTTPCCADisplay::Instance().DrawGBPoint( hit.X0()[iV], hit.X1()[iV], hit.X2()[iV], kGreen+2,(Size_t)1);
        AliHLTTPCCADisplay::Instance().DrawGBPoint( t.X()[iV], t.Y()[iV], t.Z()[iV], kBlue, 0.5 );
        cout << 0.5*atan(2*hit.ErrX12()/(hit.Err2X2() - hit.Err2X1()))[iV] << endl;
        cout << hit.X0()[iV] << " " << hit.X1()[iV] << " " << hit.X2()[iV] << " " << t.X()[iV] << " " << t.Y()[iV] << " " << t.Z()[iV] << endl;
#else
        AliHLTTPCCADisplay::Instance().DrawGBPoint(gx, gy, gz, kGreen+2,(Size_t)1);
        AliHLTTPCCADisplay::Instance().DrawGBPoint(xGl[iV], yGl[iV], zGl[iV], kBlue ,0.5);
#endif

          // cout << xGl[iV] << " " << yGl[iV] << " " << t.Z()[iV] << endl;
      }
    
      AliHLTTPCCADisplay::Instance().Ask();
    }
#endif
  }

#if !defined(PANDA_FTS)
  t.SetQPt( sfloat_v(1.e-8f), CAMath::Abs( t.QPt() ) < 1.e-8f );
#endif
  
  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;

  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

#if defined(PANDA_STT) || defined(PANDA_FTS)
  AliHLTTPCCADisplay::Instance().SetTPC( GetParameters() );
  AliHLTTPCCADisplay::Instance().DrawTPC();
  //AliHLTTPCCADisplay::Instance().DrawGBPoints();
  AliHLTTPCCADisplay::Instance().DrawGBHits( *this, kGreen+2, 0.1, 1 );
  AliHLTTPCCADisplay::Instance().SaveCanvasToFile( "Hits.pdf");
  AliHLTTPCCADisplay::Instance().Ask();
#endif
  
// //   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


#ifdef USE_DBG_TIMERS
  fGTi.Clear();
  fTi.Clear();
#endif

#ifdef USE_IDEAL_TF
  
  IdealTrackFinder();
  FitTracks();
  
#else // USE_IDEAL_TF
  
#ifdef USE_DBG_TIMERS
  TStopwatch timer;
  timer.Start(1);
#endif
  
  CATrackFinder();
#ifndef USE_CA_FIT // fitted in CATrackFinder
  FitTracks();
#endif

#ifdef USE_DBG_TIMERS
  timer.Stop();
  fGTi["tracker"] = timer;
  timer.Start(1);
#endif

#ifdef USE_DBG_TIMERS
  timer.Stop();
  fGTi["fitter "] = timer;
#endif
  
#endif // USE_IDEAL_TF
  



#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
/*
#ifdef USE_DBG_TIMERS
  static int stat_N = 0;
  stat_N++;
  
  cout << endl << " --- Timers, ms --- " << endl;
  fTi.Calc();
  fStatTi += fTi;
  L1CATFTimerInfo tmp_ti = fStatTi/0.001/stat_N; // ms
  
  tmp_ti.PrintReal();
  fStatGTi += fGTi;
  L1CATFIterTimerInfo tmp_gti = fStatGTi/0.001/stat_N; // ms
  tmp_gti.PrintReal( 1 );
#endif
 */ 
}

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);
//   }
}

bool AliHLTTPCCAGBTracker::SaveTracksInFile(string prefix) const
{
  ofstream out((prefix+"tracks.data").data());
  if ( !out.is_open() ) return 0;

  // ostream& out = cout;
  
  out << fNTracks << std::endl;
  for ( int itr = 0; itr < fNTracks; itr++ ) {
    AliHLTTPCCAGBTrack &t = fTracks[itr];

    for ( int ih = t.FirstHitRef(), i = 0; i < t.NHits(); ih++, i++ ) {
      out << fTrackHits[ih] << " ";
    }
    out << endl;

#if 0 // write local param
    out << t.InnerParam() << t.OuterParam();
#else // write general global param
#if !(defined(PANDA_STT) || defined(PANDA_FTS))
    {
      const AliHLTTPCCATrackParam &p = t.InnerParam();
      float x, y, z, px, py, pz, cov[21];
      int q;
      const bool ok = p.GetXYZPxPyPzQ( x, y, z, px, py, pz, q, cov );
      out << x << " " << y << " " << z << " " << px << " " << py << " " << pz << " " << q << endl;
      for (int i = 0, k = 0; i < 6; i++) {
        for (int j = 0; j <= i; j++, k++) {
          out << cov[k] << " ";
        }
        out << endl;
      }

      if (ok)
        out << p.Chi2() << " ";
      else
        out << -1 << " "; // invalid track
      out << p.NDF() << endl;
    }
    {
      const AliHLTTPCCATrackParam &p = t.OuterParam();
      float x, y, z, px, py, pz, cov[21];
      int q;
      const bool ok = p.GetXYZPxPyPzQ( x, y, z, px, py, pz, q, cov );
      out << x << " " << y << " " << z << " " << px << " " << py << " " << pz << " " << q << endl;
      for (int i = 0, k = 0; i < 6; i++) {
        for (int j = 0; j <= i; j++, k++) {
          out << cov[k] << " ";
        }
        out << endl;
      }

      if (ok)
        out << p.Chi2() << " ";
      else
        out << -1 << " "; // invalid track
      out << p.NDF() << endl;
    }
#endif
#endif
  }
  return 1;
}

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);
}

int AliHLTTPCCAGBTracker::ReadHitsFromFile(string prefix)
{
    ifstream ifile((prefix+"hits.data").data());
    if ( !ifile.is_open() ) return 0;
#if !defined(PANDA_STT) || 1 // all hits to read
    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] );
      assert( l.FStripP() != 0 );

#ifdef STT_ONLY
      l.SetIRow( l.IRow()+4 );
#endif
    }
#else // only forward MVD
    int Size;
    ifile >> Size; AliHLTResizableArray<AliHLTTPCCAGBHit> hitsTmp;
    hitsTmp.Resize(Size);
    fNHits = 0;

    for (int i = 0; i < Size; i++){
      AliHLTTPCCAGBHit l;
      ifile >> l;

      const int j = l.IRow();
      if (j >= 6 ) continue;
      if (l.Angle() != -1234) continue;
      hitsTmp[fNHits] = l;
      fNHits++;
    }

    if ( fNHits < 6 ) return -1;
    
    Size = fNHits;
    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

    fHits.Resize(Size);
    for (int i = 0; i < Size; i++){
      AliHLTTPCCAGBHit &l = fHits[i];
      l = hitsTmp[i];

      l.SetFStripP( &fFStrips[fNHits] );
      l.SetBStripP( &fBStrips[fNHits] );
    }
#endif

    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()
{
  
//* The necessary data constitute the set of hits. 
//* The hits are sorted by stations in process of algorithm. 
//* Hits are combined in N-plets of 2-6 hits. After that N-plets are combined into 
//* track-candidates. The track reconstruction stops when no more points are found, 
//* or when a quality check fails, based on the chi-square of the fit.
  
  int curHit = 0; // counters, used to save tracks in output format
  int curTr = 0;
  
#ifdef DRAW_CA
    AliHLTTPCCADisplay::Instance().SetTPC( GetParameters() );
    //AliHLTTPCCADisplay::Instance().SetTPCView();
    AliHLTTPCCADisplay::Instance().ClearView();
#endif
    
#ifdef USE_DBG_TIMERS
  TStopwatch timer;
  timer.Start(1);
#endif
    
    // prepare memory for tracks
  if(fTracks) delete [] fTracks;
  if(fTrackHits) delete [] fTrackHits;
#ifdef SAVE_ALL_CANDIDATES_DBG
  fTracks = new AliHLTTPCCAGBTrack[5000*100]; // TODO
  fTrackHits = new int[3000*100*AliHLTTPCCAParameters::MaxNStations]; // TODO
#else
  fTracks = new AliHLTTPCCAGBTrack[5000]; // TODO
  fTrackHits = new int[3000*AliHLTTPCCAParameters::MaxNStations]; // TODO
#endif
  fNTracks = 0;

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

  hits.Sort();

    // estimate PV
#if defined(USE_MC_PV)
  const double *pv = AliHLTTPCCAPerformance::Instance().PV(); 
  float xT = pv[0], yT = pv[1], zT = pv[2];
#else
  float xT = 0, yT = 0, zT = 0;

#if (defined(STAR_HFT) || defined(ALICE_ITS))
  {
    ITSCAHitsV hitsV(hits); // get SIMDized hits
    EstimatePV( hitsV, zT );
  }
#endif
#endif

#ifdef USE_DBG_TIMERS
  timer.Stop();
  fGTi["init  "] = timer;
  timer.Start(1);

  TStopwatch itimer;
#endif
 
    /// Find Tracks

  for( fFindIter = 0; fFindIter < fNFindIterations; ++fFindIter ) {
    // fFindIter = 1; // find secondaries in PANDA
    ITSCAHitsV hitsV(hits); // get SIMDized hits

#ifdef DRAW_CA
  AliHLTTPCCADisplay::Instance().ClearView();
  AliHLTTPCCADisplay::Instance().DrawGBHits(hitsV);
  AliHLTTPCCADisplay::Instance().SaveCanvasToFile( "DrawHits.pdf" );
  AliHLTTPCCADisplay::Instance().Ask();
#endif 
      // set parameters; TODO depend on iteration
#ifdef PANDA_FTS
  const float kCorr = 4; // correction on pulls width
   
#else
  const float kCorr = 1;//.2;

#endif
    const float kCorr2 = kCorr*kCorr;
   fPick_m = 3.*kCorr;
   fPick_r = 5.*kCorr; 
       
   
   fPickNeighbour = 3.*kCorr;
   
   TRACK_PROB_CUT = 0.01;
   TRACK_CHI2_CUT = 10*kCorr2; 


#if defined(PANDA_STT) || defined(PANDA_FTS)
    TRIPLET_CHI2_CUT = 15*kCorr2; // TMath::Prob(20,-3+1+6) = TMath::Prob(15,2) = 5e-04

#else
    TRIPLET_CHI2_CUT = 5.*kCorr2;
#endif
      // 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 ) {
      case 0: // fast primary
#if defined( PANDA_STT ) || defined( PANDA_FTS ) || defined ( ALICE_ITS )
        fMaxInvMom = 2;


#else // STAR_HFT
        fMaxInvMom = 20;
#endif

#ifdef USE_MC_PV
        fTarget = ITSCATarget( xT, yT, zT, 0.03, 0.03, fMaxInvMom/3.f, GetParameters(), 2 );
#else
#if defined( PANDA_STT )
	fTarget = ITSCATarget( xT, yT, zT, 2, 2, fMaxInvMom/3.f, GetParameters(), 3 ); // 3 so triplets with 3 tubes can have NDF=1
	//fTarget = ITSCATarget( xT, yT, zT, 1, 1, fMaxInvMom/3.f, GetParameters(), 3 ); // 3 so triplets with 3 tubes can have NDF=1 
#elif defined( PANDA_FTS )
        fTarget = ITSCATarget( xT, yT, zT, 10, 10, fMaxInvMom/3.f, GetParameters(), 0 ); // 6 tubes tracklets
#else
        fTarget = ITSCATarget( xT, yT, zT, 0.1, 0.03, fMaxInvMom/3.f, GetParameters(), 1 );
#endif
#endif
        fFindGappedTracks = false;
#ifdef STAR_HFT
        TRIPLET_CHI2_CUT = 5.*kCorr2*(1+fTarget.NDF());
#endif
        fMaxDX0 = 0;
        break;
      case 1:  // all primary
        fMaxInvMom = 20.f; // > 50 MeV tracks
        
#if defined( PANDA_STT ) || defined( PANDA_FTS )
        fTarget = ITSCATarget( xT, yT, zT, 10, 10, fMaxInvMom/3.f, GetParameters(), 0 );
        fMaxDX0 = 1;
#else
        fTarget = ITSCATarget( xT, yT, zT, 0.5, 0.5, fMaxInvMom/3.f, GetParameters(), 0 );
        fMaxDX0 = 0.1;
#endif
#ifdef STAR_HFT
        fFindGappedTracks = false;
	TRIPLET_CHI2_CUT = 5.*kCorr2*(1+fTarget.NDF());
	
#elif defined (ALICE_ITS) || defined ( PANDA_STT ) || defined( PANDA_FTS )
        fFindGappedTracks = true;
#endif
        //TRIPLET_CHI2_CUT = 5.*kCorr2*3;t
        break;
      case 2:  // all
        fMaxInvMom = 20.f;
        fTarget = ITSCATarget( xT, yT, zT, 10, 10, fMaxInvMom/3.f, GetParameters(), 0 );
        fMaxDX0 = 0.1;
        fFindGappedTracks = false;
        TRIPLET_CHI2_CUT = 5.*kCorr2*3;
        break;
    }

#ifdef USE_DBG_TIMERS
  itimer.Stop();
  fTi[fFindIter]["init  "] = itimer;
  itimer.Start(1);

  TStopwatch ptimer;
  ptimer.Start(1);
#endif
 
    // reconstruct
  ITSCANPletsV* pletsVCur,
    * pletsVNext = new ITSCANPletsV;
    
  ITSCANPletsV pletsV( NStations(), &hits );
  CreateNPlets( fTarget, hitsV, pletsV );
  
#ifdef USE_DBG_TIMERS
  itimer.Stop();
  fTi[fFindIter]["1plet "] = itimer;
  itimer.Start(1);
#endif

  pletsVCur = &pletsV;
  for( int i = 2; i <= AliHLTTPCCAParameters::MaxCellLength; i++ ) { // TODO
    if (i == 2)
      fPick = fPick_m;
    else
      fPick = fPick_r;
    
//    pletsVNext->Renew( NStations()-i+1, &hits );
    int iMin = ( i < AliHLTTPCCAParameters::LastCellLength ) ? i : AliHLTTPCCAParameters::LastCellLength;
    pletsVNext->Renew( NStations() + 1 - iMin, &hits );
    CreateNPlets( *pletsVCur, *pletsVNext );
    ITSCANPletsV* tmp = pletsVCur;
    pletsVCur = pletsVNext;
    pletsVNext = tmp;

//    cout << pletsVCur->Size() << endl;
#ifdef USE_DBG_TIMERS
    itimer.Stop();
    std::stringstream ss;
    ss << i << "plet ";
    fTi[fFindIter][ss.str()] = itimer;
    itimer.Start(1);
#endif


  }

   #ifdef DRAW_CA
    AliHLTTPCCADisplay::Instance().DrawGBNPlets(*pletsVCur);
    AliHLTTPCCADisplay::Instance().SaveCanvasToFile( "DrawCells.pdf" );
    AliHLTTPCCADisplay::Instance().Ask();
#endif 
  ITSCANPlets triplets(*pletsVCur);
  

#ifdef USE_DBG_TIMERS
  itimer.Stop();
  fTi[fFindIter]["convrt"] = itimer;
  ptimer.Stop();
  fTi[fFindIter]["plets "] = ptimer;
  
  itimer.Start(1);
#endif
    
    // for ( int i = 0; i < AliHLTTPCCAParameters::MaxNStations; i++ )
    //   cout << singletsV.OnStation( i ).size() << " "  << doubletsV.OnStation( i ).size() << " "  << tripletsV.OnStation( i ).size() << " "  << endl;
    
    fPick = fPickNeighbour;
    FindNeighbours( triplets );

#ifdef USE_DBG_TIMERS
  itimer.Stop();
  fTi[fFindIter]["nghbrs"] = itimer;
  itimer.Start(1);
#endif
    
    // for ( int i = 0; i < triplets.NStations(); i++ ) {
    //   for ( unsigned int k = 0; k < triplets.OnStation( i ).size(); k++ ) {
    //     ITSCANPlet& t = triplets.OnStation( i )[k];
    //     cout << i << " " << k << " " << t.Neighbours().size() << " " << int(t.Level()) << " " << t.Param().Chi2() << " " << t.Param().QMomentum() << " " << t.QMomentumErr() << endl;
    //   }
    //    cout << endl;
    // }
    
    ITSCATracks tracks(&hits);
    CreateTracks( triplets, tracks );

#ifdef USE_DBG_TIMERS
  itimer.Stop();
  fTi[fFindIter]["tracks"] = itimer;
  itimer.Start(1);
#endif
    
  Merge( tracks );

#ifdef USE_DBG_TIMERS
  itimer.Stop();
  fTi[fFindIter]["merger"] = itimer;
  itimer.Start(1);
#endif

      // save tracks in compact format
    const int NRTracks = tracks.size();

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

      AliHLTTPCCAGBTrack &oT = fTracks[curTr];
      oT.SetNHits( NTHits );
      oT.SetFirstHitRef( curHit );
#ifdef USE_CA_FIT
      oT.SetOuterParam( tracks[iT].Fit( hits, fTarget, GetParameters() ) );
      oT.SetInnerParam( tracks[iT].Fit( hits, fTarget, GetParameters(), false ) );
#endif
      for(int iH=0; iH < NTHits; iH++)
      {
        fTrackHits[curHit] = tracks.Hit(iH, iT).Id();
        curHit++;
      }
      curTr++;
    }
    fNTracks += NRTracks;
    
    hits.Clean(); // remove used hits
    
#ifdef USE_DBG_TIMERS
  itimer.Stop();
  fTi[fFindIter]["finish"] = itimer;
  itimer.Start(1);
#endif
  }

#ifdef USE_DBG_TIMERS
  timer.Stop();
  fGTi["iters "] = timer;
#endif
  
}

void AliHLTTPCCAGBTracker::EstimatePV( const ITSCAHitsV& all, float& zPV )
{
  //* Estimate coordinates of the primary vertex.
  
  int iS = 0;
  const float maxZ = GetParameters().MaxZ();
  const float maxPVR = 0.1; // max distance between PV and beam line
  const unsigned int N = 2*maxZ/0.01; // n bins in histo
  
  vector<float> pvHist(N,0);
  const ITSCAElementsOnStation<ITSCAHitV>& s = all.OnStation( iS );
  const ITSCAElementsOnStation<ITSCAHitV>& s2 = all.OnStation( iS+1 );
  for( unsigned int i = 0; i < s.size(); ++i ) {
    const ITSCAHitV &h = s[i];
    foreach_bit( int iV, h.IsValid() ) {
      float gx, gy, gz;
      h.GetGlobalCoor( iV, gx, gy, gz );
      AliHLTTPCCAParameters::CALocalToGlobal(float(h.X0()[iV]), float(h.X1()[iV]), float(h.X2()[iV]), float(h.Angle()[iV]), gx, gy, gz);
      float r = sqrt(gx*gx+gy*gy);

      for( unsigned int i2 = 0; i2 < s2.size(); ++i2 ) {
        const ITSCAHitV &h2 = s2[i2];
        foreach_bit( int iV2, h2.IsValid() ) {
          float gx2, gy2, gz2;
          h2.GetGlobalCoor( iV2, gx2, gy2, gz2 );
          float r2 = sqrt(gx2*gx2+gy2*gy2);

          float gz0 = -(gz2-gz)/(r2-r)*r + gz;          
	  float gx0 = -(gx2-gx)/(r2-r)*r + gx;
          float gy0 = -(gy2-gy)/(r2-r)*r + gy; 
          if ( abs(gz0) < maxZ && abs(gx0) < maxPVR && abs(gy0) < maxPVR )
            pvHist[(gz0/maxZ+1)*N/2]++; 
        }
      }
    }

  }

#ifdef DRAW_CA
  const double *pv = AliHLTTPCCAPerformance::Instance().PV(); // dbg
  AliHLTTPCCADisplay::Instance().DrawGBHits(all);
  AliHLTTPCCADisplay::Instance().DrawPVHisto( pvHist, GetParameters() );
  AliHLTTPCCADisplay::Instance().DrawGBPoint(pv[0],pv[1],pv[2],2,0.5);
  AliHLTTPCCADisplay::Instance().SaveCanvasToFile( "DrawPVHisto.pdf" );
  AliHLTTPCCADisplay::Instance().Ask();
#endif
  
  float max = -1;
  int maxI = -1;
  for( unsigned int i = 0; i < pvHist.size(); ++i ) {
    if ( max < pvHist[i] ) {
      max = pvHist[i];
      maxI = i;
    }
  }

  zPV = (2.f*maxI/N-1)*maxZ;
}

void AliHLTTPCCAGBTracker::CreateNPlets( const ITSCATarget& target, const ITSCAHitsV& hits, ITSCANPletsV& singlets )
{
  //* Hits could be combined into all possible singlets consisting of target and nearby hit.
  
  for( char iS = 0; iS < NStations(); ++iS ) {
    singlets.OnStation( iS ) = Combine( target, hits.OnStation( iS ) );
  }
// #ifdef DRAW_CA
//   AliHLTTPCCADisplay::Instance().DrawGBNPlets(singlets);
//   AliHLTTPCCADisplay::Instance().Ask();
// #endif
}

void AliHLTTPCCAGBTracker::CreateNPlets( const ITSCANPletsV& doublets, ITSCANPletsV& triplets )
{
  //* At start, hits are combined into all possible groups of 2 consecutive hits.
  //* These groups are named doublets and pairs of doublets are then combined into triplets 
  //* if they have a common hit. This process continues until we obtain groups of 6 hits 
  //* in N-plets. To obtain the optimal combinations of doublets for the construction 
  //* of triplet one uses the sorted list of hits. 
  //* In such way the unnecessary hits are excluded.
  
  if ( fFindGappedTracks ) { // CHECKME for N > 3
    for( int iS = 0; iS < doublets.NStations()-2; ++iS ) { // TODO Panda
      if ( doublets.OnStation( iS ).size() <= 0 ) continue;
      if ( doublets.OnStation( iS )[0].N() >= GetParameters().Station( iS ).CellLength ) {
        triplets.OnStation( iS ) = doublets.OnStation( iS );
        continue; // already have enough hits
      }
      
      triplets.OnStation( iS ) = Combine( doublets.OnStation( iS ), doublets.OnStation( iS + 1 ) ) + Combine( doublets.OnStation( iS ), doublets.OnStation( iS + 2 ) );
    }
    int iS = doublets.NStations()-2;
    if ( doublets.OnStation( iS ).size() <= 0 ) return;
    if ( doublets.OnStation( iS )[0].N() >= GetParameters().Station( iS ).CellLength ) {
      triplets.OnStation( iS ) = doublets.OnStation( iS );
      return; // already have enough hits
    }
    triplets.OnStation( iS ) = Combine( doublets.OnStation( iS ), doublets.OnStation( iS + 1 ) );
  }
  else
    for( int iS = 0; iS < doublets.NStations()-1; ++iS ) {
      if ( doublets.OnStation( iS ).size() <= 0 ) continue;
      if ( doublets.OnStation( iS )[0].N() >= GetParameters().Station( iS ).CellLength ) {
        triplets.OnStation( iS ) = doublets.OnStation( iS );
        continue; // already have enough hits
      }
      triplets.OnStation( iS ) = Combine( doublets.OnStation( iS ), doublets.OnStation( iS + 1 ) );
// #ifdef DRAW_CA
//       cout << iS << endl;
//       AliHLTTPCCADisplay::Instance().DrawGBNPlets(triplets);
//       AliHLTTPCCADisplay::Instance().SaveCanvasToFile( "DrawNPlets.pdf" );
//       AliHLTTPCCADisplay::Instance().Ask();
// #endif
    }
// #ifdef DRAW_CA
//   AliHLTTPCCADisplay::Instance().DrawGBNPlets(triplets);
//   AliHLTTPCCADisplay::Instance().SaveCanvasToFile( "DrawNPlets.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( ITSCANPlets& triplets )
{
  //* Identification of neighbours for each N-plet, 
  //* that is of N-plets which are located on the adjacent stations.
  
  for( int iS = triplets.NStations() - 2; iS >= 0; --iS ) { // CHECKME
    ITSCAElementsOnStation<ITSCANPlet>& ts1 = triplets.OnStation( iS );
    
    for( unsigned int iT1 = 0; iT1 < ts1.size(); ++iT1 ) {
      ITSCANPlet& t1 = ts1[iT1];

      if ( t1.ISta(1) >= triplets.NStations() ) continue; // triplets can't start from this station
      ITSCAElementsOnStation<ITSCANPlet>& ts2 = triplets.OnStation( t1.ISta(1) );
      
      char maxLevel = -1; // maxLevel of neighbour triplets
      vector< pair<float,unsigned int> > neighCands; // save neighbour candidates
      for( unsigned int iT2 = 0; iT2 < ts2.size(); ++iT2 ) {
        const ITSCANPlet& t2 = ts2[iT2];
        float chi2;
        if( !t1.IsRightNeighbour( fPick, t2, chi2 ) ) continue;

        if ( maxLevel <  t2.Level() ) maxLevel = t2.Level();
        if ( maxLevel == t2.Level() ) neighCands.push_back(pair<float,unsigned int>(chi2 + t2.Chi2Level(),iT2));
      }
      t1.Level() = maxLevel + 1;

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

        if ( maxLevel == t2.Level() ) {
          t1.Neighbours().push_back( neighCands[iN] );
        }
      }
      sort( t1.Neighbours().begin(), t1.Neighbours().end() );
      if ( t1.NNeighbours() ) {
        t1.Chi2Level() = t1.Chi2Neighbours(0);
#ifdef DRIFT_TUBES // check for others detectors
        const pair<float,unsigned int> tmp = t1.Neighbours()[0]; // leave only one. CHECKME for 1000tracks events
        t1.Neighbours().clear();
        t1.Neighbours().push_back( tmp );
#endif
      }
    } // iTrip1

    //sort( ts1.begin(), ts1.end(), ITSCANPlet::compare );
  } // iStation
}

void AliHLTTPCCAGBTracker::CreateTracks( const ITSCANPlets& triplets, ITSCATracks& tracks )
{
  //* Creation of tracks is performed by linking the track segments (N-plets)
  //* of maximum length. The chi-square estimate for each track-candidate
  //* is used as a selection criteria. After that transport and fitting of tracks are performed.
  
  const int Nlast = AliHLTTPCCAParameters::LastCellLength; // N hits in the rightmost triplet

#ifdef DRIFT_TUBES
  int min_level = 1; // min level to start triplet. So min track length = min_level+3.
#else
  int min_level = 0;
#endif
    // collect consequtive: the longest tracks, shorter, more shorter and so on
  for (int ilev = NStations()-Nlast; ilev >= min_level; ilev--){ // choose length
    ITSCATracks vtrackcandidate( tracks.HitsRef() );
      
      //  how many levels to check
    const unsigned char min_best_l = (ilev > min_level) ? ilev-1 : min_level; // 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()-Nlast-ilev; istaF++ ){
      const ITSCAElementsOnStation<ITSCANPlet>& tsF = triplets.OnStation( istaF );

      for( unsigned int iTrip=0; iTrip < tsF.size(); iTrip++ ){
        const ITSCANPlet* 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
        }
        else
          if ( tripF->Level() < min_best_l ) continue;

        ITSCATrack curr_tr;

        bool isUsed = false;
        for( int i = 0; i < tripF->N(); i++ ) {
          if( triplets.OnStation( tripF->ISta(0) ).GetHit( i, iTrip ).IsUsed() ) {
            isUsed = true;
            break;
          }
          curr_tr.AddHit( tripF->IHit(i) );
        }
        if (isUsed) continue;
        curr_tr.Chi2() = tripF->Param().Chi2();
        
        ITSCATrack best_tr = curr_tr;
        unsigned int nCalls = 0;
        FindBestCandidate(istaF, best_tr, iTrip, curr_tr, min_best_l, triplets, nCalls );

        if ( best_tr.Level() < min_best_l ) continue;
        if ( best_tr.NHits() < AliHLTTPCCAParameters::MinimumHitsForRecoTrack ) continue;
        
        best_tr.Fit( *triplets[istaF].HitsRef(), fTarget, GetParameters() ); ///
        if (TMath::Prob( best_tr.Chi2(), best_tr.NDF() ) < TRACK_PROB_CUT) continue; ///
	  

#if !defined(SAVE_ALL_CANDIDATES_DBG)
#ifndef DRIFT_TUBES // check for others detectors
        //best_tr.Fit( *triplets[istaF].HitsRef(), fTarget, GetParameters() );
      // if (TMath::Prob( best_tr.Chi2(), best_tr.NDF() ) < TRACK_PROB_CUT) continue;

#endif // DRIFT_TUBES
#endif // !defined(SAVE_ALL_CANDIDATES_DBG)
//       if ( best_tr.Fit( *tracks.HitsRef(), fTarget, GetParameters(), tripF->QMomentum() ).QPt() > 0.5*fMaxInvMom ) continue; // fit to determine Chi2 and NDF

        vtrackcandidate.push_back(best_tr);
         // best_tr.SetHitsAsUsed(*tracks.HitsRef()); TODO
         // tracks.push_back(best_tr);
      }
    } // istaF

      // select and save best candidates
    vtrackcandidate.SelectAndSaveTracks( tracks );
  } // ilev

#ifdef DRAW_CA
  AliHLTTPCCADisplay::Instance().ClearView();
  // AliHLTTPCCADisplay::Instance().DrawGBHits(*tracks.HitsRef());
    AliHLTTPCCADisplay::Instance().DrawGBHits( *this, kGreen+2, 0.03, 1 );
  AliHLTTPCCADisplay::Instance().DrawGBTracks(tracks);
  AliHLTTPCCADisplay::Instance().SaveCanvasToFile( "DrawTracks.pdf" );
  AliHLTTPCCADisplay::Instance().Ask();
#endif
}

// --------- Merger ---------


void AliHLTTPCCAGBTracker::InvertCholetsky(float a[15]) const
{
  float d[5], uud, u[5][5];
  for(int i=0; i<5; i++) 
  {
    d[i]=0.f;
    for(int j=0; j<5; j++) 
      u[i][j]=0.;
  }

  for(int i=0; i<5; i++)
  {
    uud=0.;
    for(int j=0; j<i; j++) 
      uud += u[j][i]*u[j][i]*d[j];
    uud = a[i*(i+3)/2] - uud;

    if(fabs(uud)<1.e-12) uud = 1.e-12;
    d[i] = uud/fabs(uud);
    u[i][i] = sqrt(fabs(uud));

    for(int j=i+1; j<5; j++) 
    {
      uud = 0.;
      for(int k=0; k<i; k++)
        uud += u[k][i]*u[k][j]*d[k];
      uud = a[j*(j+1)/2+i] - uud;
      u[i][j] = d[i]/u[i][i]*uud;
    }
  }

  float u1[5];

  for(int i=0; i<5; i++)
  {
    u1[i] = u[i][i];
    u[i][i] = 1.f/u[i][i];
  }
  for(int i=0; i<4; i++)
  {
    u[i][i+1] = - u[i][i+1]*u[i][i]*u[i+1][i+1];
  }
  for(int i=0; i<3; i++)
  {
    u[i][i+2] = u[i][i+1]*u1[i+1]*u[i+1][i+2]-u[i][i+2]*u[i][i]*u[i+2][i+2];
  }
  for(int i=0; i<2; i++)
  {
    u[i][i+3] = u[i][i+2]*u1[i+2]*u[i+2][i+3] - u[i][i+3]*u[i][i]*u[i+3][i+3];
    u[i][i+3] -= u[i][i+1]*u1[i+1]*(u[i+1][i+2]*u1[i+2]*u[i+2][i+3] - u[i+1][i+3]);
  }
  u[0][4] = u[0][2]*u1[2]*u[2][4] - u[0][4]*u[0][0]*u[4][4];
  u[0][4] += u[0][1]*u1[1]*(u[1][4] - u[1][3]*u1[3]*u[3][4] - u[1][2]*u1[2]*u[2][4]);
  u[0][4] += u[3][4]*u1[3]*(u[0][3] - u1[2]*u[2][3]*(u[0][2] - u[0][1]*u1[1]*u[1][2]));

  for(int i=0; i<5; i++)
    a[i+10] = u[i][4]*d[4]*u[4][4];
  for(int i=0; i<4; i++)
    a[i+6] = u[i][3]*u[3][3]*d[3] + u[i][4]*u[3][4]*d[4];
  for(int i=0; i<3; i++)
    a[i+3] = u[i][2]*u[2][2]*d[2] + u[i][3]*u[2][3]*d[3] + u[i][4]*u[2][4]*d[4];
  for(int i=0; i<2; i++)
    a[i+1] = u[i][1]*u[1][1]*d[1] + u[i][2]*u[1][2]*d[2] + u[i][3]*u[1][3]*d[3] + u[i][4]*u[1][4]*d[4];
  a[0] = u[0][0]*u[0][0]*d[0] + u[0][1]*u[0][1]*d[1] + u[0][2]*u[0][2]*d[2] + u[0][3]*u[0][3]*d[3] + u[0][4]*u[0][4]*d[4];
}

void AliHLTTPCCAGBTracker::MultiplySS(float const C[15], float const V[15], float K[5][5]) const
{
//*multiply 2 symmetric matricies
  K[0][0] = C[0]*V[ 0] + C[1]*V[ 1] + C[3]*V[ 3] + C[6]*V[ 6] + C[10]*V[10];
  K[0][1] = C[0]*V[ 1] + C[1]*V[ 2] + C[3]*V[ 4] + C[6]*V[ 7] + C[10]*V[11];
  K[0][2] = C[0]*V[ 3] + C[1]*V[ 4] + C[3]*V[ 5] + C[6]*V[ 8] + C[10]*V[12];
  K[0][3] = C[0]*V[ 6] + C[1]*V[ 7] + C[3]*V[ 8] + C[6]*V[ 9] + C[10]*V[13];
  K[0][4] = C[0]*V[10] + C[1]*V[11] + C[3]*V[12] + C[6]*V[13] + C[10]*V[14];

  K[1][0] = C[1]*V[ 0] + C[2]*V[ 1] + C[4]*V[ 3] + C[7]*V[ 6] + C[11]*V[10];
  K[1][1] = C[1]*V[ 1] + C[2]*V[ 2] + C[4]*V[ 4] + C[7]*V[ 7] + C[11]*V[11];
  K[1][2] = C[1]*V[ 3] + C[2]*V[ 4] + C[4]*V[ 5] + C[7]*V[ 8] + C[11]*V[12];
  K[1][3] = C[1]*V[ 6] + C[2]*V[ 7] + C[4]*V[ 8] + C[7]*V[ 9] + C[11]*V[13];
  K[1][4] = C[1]*V[10] + C[2]*V[11] + C[4]*V[12] + C[7]*V[13] + C[11]*V[14];

  K[2][0] = C[3]*V[ 0] + C[4]*V[ 1] + C[5]*V[ 3] + C[8]*V[ 6] + C[12]*V[10];
  K[2][1] = C[3]*V[ 1] + C[4]*V[ 2] + C[5]*V[ 4] + C[8]*V[ 7] + C[12]*V[11];
  K[2][2] = C[3]*V[ 3] + C[4]*V[ 4] + C[5]*V[ 5] + C[8]*V[ 8] + C[12]*V[12];
  K[2][3] = C[3]*V[ 6] + C[4]*V[ 7] + C[5]*V[ 8] + C[8]*V[ 9] + C[12]*V[13];
  K[2][4] = C[3]*V[10] + C[4]*V[11] + C[5]*V[12] + C[8]*V[13] + C[12]*V[14];

  K[3][0] = C[6]*V[ 0] + C[7]*V[ 1] + C[8]*V[ 3] + C[9]*V[ 6] + C[13]*V[10];
  K[3][1] = C[6]*V[ 1] + C[7]*V[ 2] + C[8]*V[ 4] + C[9]*V[ 7] + C[13]*V[11];
  K[3][2] = C[6]*V[ 3] + C[7]*V[ 4] + C[8]*V[ 5] + C[9]*V[ 8] + C[13]*V[12];
  K[3][3] = C[6]*V[ 6] + C[7]*V[ 7] + C[8]*V[ 8] + C[9]*V[ 9] + C[13]*V[13];
  K[3][4] = C[6]*V[10] + C[7]*V[11] + C[8]*V[12] + C[9]*V[13] + C[13]*V[14];

  K[4][0] = C[10]*V[ 0] + C[11]*V[ 1] + C[12]*V[ 3] + C[13]*V[ 6] + C[14]*V[10];
  K[4][1] = C[10]*V[ 1] + C[11]*V[ 2] + C[12]*V[ 4] + C[13]*V[ 7] + C[14]*V[11];
  K[4][2] = C[10]*V[ 3] + C[11]*V[ 4] + C[12]*V[ 5] + C[13]*V[ 8] + C[14]*V[12];
  K[4][3] = C[10]*V[ 6] + C[11]*V[ 7] + C[12]*V[ 8] + C[13]*V[ 9] + C[14]*V[13];
  K[4][4] = C[10]*V[10] + C[11]*V[11] + C[12]*V[12] + C[13]*V[13] + C[14]*V[14];
}

void AliHLTTPCCAGBTracker::MultiplyMS(float const C[5][5], float const V[15], float K[15]) const
{
//*multiply symmetric and nonsymmetric matricies
  K[0] = C[0][0]*V[0] + C[0][1]*V[1] + C[0][2]*V[3] + C[0][3]*V[6] + C[0][4]*V[10];

  K[1] = C[1][0]*V[0] + C[1][1]*V[1] + C[1][2]*V[3] + C[1][3]*V[6] + C[1][4]*V[10];
  K[2] = C[1][0]*V[1] + C[1][1]*V[2] + C[1][2]*V[4] + C[1][3]*V[7] + C[1][4]*V[11];

  K[3] = C[2][0]*V[0] + C[2][1]*V[1] + C[2][2]*V[3] + C[2][3]*V[6] + C[2][4]*V[10];
  K[4] = C[2][0]*V[1] + C[2][1]*V[2] + C[2][2]*V[4] + C[2][3]*V[7] + C[2][4]*V[11];
  K[5] = C[2][0]*V[3] + C[2][1]*V[4] + C[2][2]*V[5] + C[2][3]*V[8] + C[2][4]*V[12];

  K[6] = C[3][0]*V[0] + C[3][1]*V[1] + C[3][2]*V[3] + C[3][3]*V[6] + C[3][4]*V[10];
  K[7] = C[3][0]*V[1] + C[3][1]*V[2] + C[3][2]*V[4] + C[3][3]*V[7] + C[3][4]*V[11];
  K[8] = C[3][0]*V[3] + C[3][1]*V[4] + C[3][2]*V[5] + C[3][3]*V[8] + C[3][4]*V[12];
  K[9] = C[3][0]*V[6] + C[3][1]*V[7] + C[3][2]*V[8] + C[3][3]*V[9] + C[3][4]*V[13];

  K[10] = C[4][0]*V[ 0] + C[4][1]*V[ 1] + C[4][2]*V[ 3] + C[4][3]*V[ 6] + C[4][4]*V[10];
  K[11] = C[4][0]*V[ 1] + C[4][1]*V[ 2] + C[4][2]*V[ 4] + C[4][3]*V[ 7] + C[4][4]*V[11];
  K[12] = C[4][0]*V[ 3] + C[4][1]*V[ 4] + C[4][2]*V[ 5] + C[4][3]*V[ 8] + C[4][4]*V[12];
  K[13] = C[4][0]*V[ 6] + C[4][1]*V[ 7] + C[4][2]*V[ 8] + C[4][3]*V[ 9] + C[4][4]*V[13];
  K[14] = C[4][0]*V[10] + C[4][1]*V[11] + C[4][2]*V[12] + C[4][3]*V[13] + C[4][4]*V[14];
}

void AliHLTTPCCAGBTracker::MultiplySR(float const C[15], float const r_in[5], float r_out[5]) const
{
//*multiply vector and symmetric matrix
  r_out[0] = r_in[0]*C[ 0] + r_in[1]*C[ 1] + r_in[2]*C[ 3] +r_in[3]*C[ 6] + r_in[4]*C[10];
  r_out[1] = r_in[0]*C[ 1] + r_in[1]*C[ 2] + r_in[2]*C[ 4] +r_in[3]*C[ 7] + r_in[4]*C[11];
  r_out[2] = r_in[0]*C[ 3] + r_in[1]*C[ 4] + r_in[2]*C[ 5] +r_in[3]*C[ 8] + r_in[4]*C[12];
  r_out[3] = r_in[0]*C[ 6] + r_in[1]*C[ 7] + r_in[2]*C[ 8] +r_in[3]*C[ 9] + r_in[4]*C[13];
  r_out[4] = r_in[0]*C[10] + r_in[1]*C[11] + r_in[2]*C[12] +r_in[3]*C[13] + r_in[4]*C[14];
}

void AliHLTTPCCAGBTracker::FilterTracks(float const r[5], float const C[15], float const m[5], float const V[15], float R[5], float W[15], float &chi2) const
{
  //* Track filtering is realized with the Kalman Filter, 
  //* which allows one to obtain optimal estimate of track's parameters.
  
  float S[15];
  for(int i=0; i<15; i++)
  {
    W[i] = C[i];
    S[i] = C[i] + V[i];
  }
  for(int i=0; i<5; i++)
    R[i] = r[i];

  InvertCholetsky(S);
  
  float K[5][5];
  MultiplySS(C,S,K);
  float dzeta[5];
  for(int i=0; i<5; i++) dzeta[i] = m[i] - r[i];
  float KC[15];
  MultiplyMS(K,C,KC);
  for(int i=0; i< 15; i++)
    W[i] -= KC[i];

  float kd;
  for(int i=0; i<5; i++)
  {
    kd = 0.f;
    for(int j=0; j<5; j++)
      kd += K[i][j]*dzeta[j];
    R[i] += kd;
  }
  float S_dzeta[5];
  MultiplySR(S, dzeta, S_dzeta);
  chi2 = dzeta[0]*S_dzeta[0] + dzeta[1]*S_dzeta[1] + dzeta[2]*S_dzeta[2] + dzeta[3]*S_dzeta[3] + dzeta[4]*S_dzeta[4];
}

void AliHLTTPCCAGBTracker::Merge( ITSCATracks& tracks )
{
  //* Linking of track segments into tracks considering the distance between the end 
  //* of the first segment and the beginning of the second segment.
  
  const int NTracksS = tracks.size();
  vector< vector< pair<float,int> > > InNeighbour(NTracksS);
  vector< vector< pair<float,int> > > OutNeighbour(NTracksS);
  
  for ( int iT1 = 0; iT1 < NTracksS; iT1++ ) {
    ITSCATrack& t1 = tracks[iT1];
    if ( t1.NHits() <= 0 ) continue;

    for ( int iT2 = 0; iT2 < NTracksS; iT2++ ) {
      ITSCATrack& t2 = tracks[iT2];
      if ( t2.NHits() <= 0 ) continue;

      const float xDiff = (*tracks.HitsRef())[t2.IHits().front()].X0() - (*tracks.HitsRef())[t1.IHits().back()].X0();
      if ( xDiff <= 0 ) continue;

      AliHLTTPCCATrackParam p1 = t1.Fit( *tracks.HitsRef(), fTarget, GetParameters() );

      AliHLTTPCCATrackParam p2 = t2.Fit( *tracks.HitsRef(), fTarget, GetParameters() ); // TODO: fit in advance. CHECKME why fit backward doesn't help. CHECKME why fit without target doesn't work
      // if ( t2.NHits() >= t1.NHits() ) // CHECK me: why it is better to always use p2
        p2.Transport( (*tracks.HitsRef())[t1.IHits().back()], GetParameters() );
        //p2.TransportToParam( p1, GetParameters().cBz() );
      // else
      //   p1.Transport( (*tracks.HitsRef())[t2.IHits().back()], GetParameters().cBz() );

      float C[15], r[5], chi2(0);
      FilterTracks( p1.Par(), p1.Cov(), p2.Par(), p2.Cov(), r, C, chi2);

      if ( chi2 < -1 || 20 < chi2 ) continue; // TMath::Prob(20,5) = 1.25e-03

      chi2 += 10*xDiff; // xDiff is more important than chi2 - this is an empiric choice

      OutNeighbour[iT1].push_back( pair<float,int>( chi2, iT2 ) );
      InNeighbour[iT2].push_back( pair<float,int>( chi2, iT1 ) );
  
    } // iT2
  } // iT1

    // sort by chi2
  for ( int iT1 = 0; iT1 < NTracksS; iT1++ ) {
      sort( InNeighbour[iT1].begin(), InNeighbour[iT1].end() );
      sort( OutNeighbour[iT1].begin(), OutNeighbour[iT1].end() );
  }

    // combine best neighbours. TODO: speed up by sorting all pairs together by chi2
  bool allPairsFound = false;
  while( !allPairsFound ) {
    allPairsFound = true;

    // { // dbg
    //   cout << " new " << endl;
    //   for ( int iT1 = 0; iT1 < NTracksS; iT1++ ) {
    //     for( unsigned int iN = 0; iN < InNeighbour[iT1].size(); iN++ ) {
    //       cout << iT1 << " " << iN << " I " << InNeighbour[iT1][iN].first << " " << InNeighbour[iT1][iN].second << endl;
    //     }
    //     for( unsigned int iN = 0; iN < OutNeighbour[iT1].size(); iN++ ) {
    //       cout << iT1 << " " <<  iN << " O " <<  OutNeighbour[iT1][iN].first << " " << OutNeighbour[iT1][iN].second << endl;
    //     }
    //   }
    // }
    
    for ( int iT1 = 0; iT1 < NTracksS; iT1++ ) {
      ITSCATrack& t1 = tracks[iT1];
      if ( t1.NHits() <= 0 ) continue;
  
        // find best unused outer track
      int iT2 = -1;
      for( unsigned int iN = 0; iN < OutNeighbour[iT1].size(); iN++ ) {
        iT2 = OutNeighbour[iT1][iN].second;
        if( iT2 >= 0 ) {
          break;
        }
      }
      if (iT2 < 0) continue;

        // find best unused inner track for outer track
      int iT21 = -1;
      for( unsigned int iN = 0; iN < InNeighbour[iT2].size(); iN++ ) {
        iT21 = InNeighbour[iT2][iN].second;
        if( iT21 >= 0 ) {
          break;
        }
      }
      assert(iT21 >= 0); // at least iT1 should be found
    
      if ( iT1 != iT21 ) {
        allPairsFound = false;
        continue; // find them at the next iteration
      }
      ITSCATrack& t2 = tracks[iT2];

        // atach track
      for ( int ih = 0; ih < t2.NHits(); ih++ ) {
        t1.AddHit( t2.IHits()[ih] ); 
      } 
      t2.IHits().clear();
     
        // clean connections
      for( unsigned int iN = 0; iN < OutNeighbour[iT1].size(); iN++ ) {
        const int iT22 = OutNeighbour[iT1][iN].second;
        if ( iT22 < 0 ) continue;
        for( unsigned int iN2 = 0; iN2 < InNeighbour[iT22].size(); iN2++ ) {
          if( InNeighbour[iT22][iN2].second == iT1 ) {
            InNeighbour[iT22][iN2].second = -1;
            break;
          }
        }
      }
      for( unsigned int iN = 0; iN < InNeighbour[iT2].size(); iN++ ) {
        const int iT22 = InNeighbour[iT2][iN].second;
        if ( iT22 < 0 ) continue;
        for( unsigned int iN2 = 0; iN2 < OutNeighbour[iT22].size(); iN2++ ) {
          if( OutNeighbour[iT22][iN2].second == iT2 ) {
            OutNeighbour[iT22][iN2].second = -1;
            break;
          }
        }
      }
      for( unsigned int iN = 0; iN < OutNeighbour[iT2].size(); iN++ ) {
        const int iT22 = OutNeighbour[iT2][iN].second;
        if ( iT22 < 0 ) continue;
        for( unsigned int iN2 = 0; iN2 < InNeighbour[iT22].size(); iN2++ ) {
          if( InNeighbour[iT22][iN2].second == iT2 ) {
            InNeighbour[iT22][iN2].second = iT1;
            break;
          }
        }
      }
      OutNeighbour[iT1] = OutNeighbour[iT2];
      InNeighbour[iT2].clear();
      OutNeighbour[iT2].clear();
              // check the newly created track
      iT1--;
    }
  } // allPairsFound
  
    // remove tracks, which were atached to other tracks

  ITSCATracks tracks_saved = tracks;
  tracks.clear();
  for ( int iT1 = 0; iT1 < NTracksS; iT1++ ) {
    ITSCATrack& t1 = tracks_saved[iT1];
    if (t1.NHits() != 0)
      tracks.push_back(t1);
  }
  // currently is not needed
}

ITSCAElementsOnStation<ITSCANPletV> AliHLTTPCCAGBTracker::Combine( const ITSCATarget& t, const ITSCAElementsOnStation<ITSCAHitV>& h)
{
  //* Function of target and successive singlet combination.
  
  ITSCAElementsOnStation<ITSCANPletV> r;

  for( unsigned short iH = 0; iH < h.size(); ++iH ) {
    const ITSCAHitV& hit = h[iH];
    AliHLTTPCCATrackParamVector param;
#ifdef START_FROM_TARGET
    param.InitByTarget(t);
    param.InitDirection( hit.X0() - t.X0(), hit.X1() - t.X1(), hit.X2() - t.X2() );
    param.SetAngle( hit.Angle() );

    sfloat_m active = hit.IsValid();
    active &= param.Transport( hit, GetParameters(), active );
    active &= param.Filter( hit, GetParameters(), active );
#else
    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 );
#endif

    active &= param.Transport( hit.IStations() + short_v(1), GetParameters(), active );
    r.push_back( ITSCANPletV( ushort_v::IndexesFromZero() + iH*ushort_v::Size, short_v(h.IStation()), param, active ) );
  }

  return r;
}

#ifdef STAR_HFT
// #define USE_HITSSORT
#elif defined (DRIFT_TUBES)
  //#define USE_HITSSORT // comment to try all combinations
#else
  #define USE_HITSSORT // comment to try all combinations
#endif

ITSCAElementsOnStation<ITSCANPletV> AliHLTTPCCAGBTracker::Combine( const ITSCAElementsOnStation<ITSCANPletV>& a, const ITSCAElementsOnStation<ITSCANPletV>& b )
{
  //* Function of N-plets combination into longer groups with sorting.
  
  ITSCAElementsOnStation<ITSCANPletV> r( a.HitsRef() );
  r.SetStation( a.IStation() );

  if ( a.size() <= 0 ) return r;
  const int N = a[0].N() + 1;

  if ( N > 2 ) {
    r.reserve(a.size()); // TODO if NDF < 0
 
    for( unsigned int iD1 = 0; iD1 < a.size(); ++iD1 ) {
      const ITSCANPletV& D1 = a[iD1];
      sfloat_m valid1G = D1.IsValid();
      valid1G &= D1.IHit(1).s == b.IStation(); // in case of gapped nplets can be different
      foreach_bit( unsigned int iV1, valid1G ) { // make the code scalar to use FirstElementIByHit0
      const sfloat_m valid1 = sfloat_m(ushort_v(Vc::IndexesFromZero) == iV1);

      const int iH = D1.IHit(1).e[valid1.firstOne()]; // TODO vectorize
      assert( iH < b.HitsRef()->OnStation(b.IStation()).size() );
      
      int iStartS2 = b.FirstElementIByHit0(iH);
      int iEndS2 = b.FirstElementIByHit0(iH+1)-1;

      if ( iStartS2 < 0 ) continue;
      if ( iEndS2 < iStartS2 ) continue;
      unsigned int iStartS2V = iStartS2/sfloat_v::Size;
      unsigned short iStartS2E = iStartS2%sfloat_v::Size;
      unsigned int iEndS2V = iEndS2/sfloat_v::Size;
      unsigned short iEndS2E = iEndS2%sfloat_v::Size;
      // if (b.size() <= 0 ) continue;
      // unsigned int iStartS2V = 0;
      // unsigned short iStartS2E = 0;
      // unsigned int iEndS2V = b.size() - 1;
      // unsigned short iEndS2E = 7;
      
      for( unsigned int iD2 = iStartS2V; iD2 <= iEndS2V; ++iD2 ) { // try each 2nd hit from 2nd doubletVector with all 1st doublets
        const ITSCANPletV& D2 = b[iD2];
        const sfloat_m valid2 = D2.IsValid() &&
          (static_cast<sfloat_m>(ushort_v(Vc::IndexesFromZero) >= iStartS2E) || sfloat_m(iD2 > iStartS2V)) &&
          (static_cast<sfloat_m>(ushort_v(Vc::IndexesFromZero) <= iEndS2E  ) || sfloat_m(iD2 < iEndS2V  ));

        foreach_bit( unsigned int iV, valid2 ) { // try each hit from 2nd doublet with all 1st doublets
          sfloat_m active = valid1;

          active &= D1.IsRightNeighbour( D2, iV );
          if ( active.isEmpty() ) continue;
      
          const ITSCAHit& hit = b.GetHit( iV, N-2, iD2 );
      
          AliHLTTPCCATrackParamVector param = D1.Param();

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

          active &= IsEqual( param, hit );
          if ( active.isEmpty() ) continue;
      
#ifdef STAR_HFT // don't need refit if tracks are selected by fit-chi2
            //ITSCANPletV<N> trip = ITSCANPletV<N>( D1, D2, iV, param, active );
            //Refit( trip, *a.HitsRef() );
            //param = param;
            //active &= param.Chi2() < TRIPLET_CHI2_CUT * static_cast<sfloat_v>(param.NDF());
            //        active &= abs(param.QPt()) < fMaxInvMom*3;
            //if ( active.isEmpty() ) continue;
#endif
          if (fFindIter == 0)
          {
            active &= param.Filter( hit, GetParameters(), active, TRIPLET_CHI2_CUT );
            if ( active.isEmpty() ) continue;
          }
          else {
            active &= param.Filter( hit, GetParameters(), active );
            ITSCANPletV trip = ITSCANPletV( D1, D2, iV, param, active );
            active &= Refit( trip, *a.HitsRef() );
            active &= trip.Param().Chi2() < TRIPLET_CHI2_CUT; 
            if ( active.isEmpty() ) continue;
            param = trip.Param();
          }

          //active &= param.Transport( short(hit.IStation() + 1), GetParameters(), active );
          //if ( active.isEmpty() ) continue; // dbg
          r.push_back( ITSCANPletV( D1, D2, iV, param, active ) );
//          foreach_bit( unsigned int iV1, active ) {
            const int pos =  (r.size()-1)*sfloat_v::Size + iV1;
            if (r.FirstElementIByHit0()[D1.IHit(0)[iV1].e] == -1)
              r.FirstElementIByHit0()[D1.IHit(0)[iV1].e] = pos;
            r.FirstElementIByHit0()[D1.IHit(0)[iV1].e + 1] = pos+1; // save the end
//          }
        } // iV
      } // iD2
      } // iV1
    } // iD1
  }
  else { // doublet
    // r.reserve(a.size()*5); // TODO if NDF < 0
    unsigned int iStartS2 = 0; // save first good singet2 for last singlet1
    for( unsigned int iS1 = 0; iS1 < a.size(); ++iS1 ) {
      const ITSCANPletV& s1 = a[iS1];
      const sfloat_m valid1G = s1.IsValid();
      foreach_bit( unsigned int iV1, valid1G ) {
        const sfloat_m valid1 = sfloat_m(ushort_v(Vc::IndexesFromZero) == iV1);

        // find iStartS2
      unsigned int iStartS2V = iStartS2/sfloat_v::Size;
      unsigned int iStartS2E = iStartS2%sfloat_v::Size;
#ifdef USE_HITSSORT 
      bool firstFound = false;
      for( unsigned int iS2 = iStartS2V; iS2 < b.size() && !firstFound; ++iS2 ) {
        const ITSCANPletV& 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;
#if defined( PANDA_STT ) || defined( PANDA_FTS )
          sfloat_m inRange;
          if ( hit.IStation() > AliHLTTPCCAParameters::NMVDStations ) {
            const sfloat_v dx1 = hit.X1Corrected(param) - param.X1() + 2*fMaxDX0*abs(param.Tx1());
            const sfloat_v dx1_est2 = Pick2*(abs(param.Err2X1() + hit.Err2X1()));
            inRange = (dx1*dx1 <= dx1_est2) || (dx1 > 0);
          }
          else if ( hit.IStation() < AliHLTTPCCAParameters::NMVDStations ) {
            const sfloat_v dx2 = hit.X2() - param.X2() + 2*fMaxDX0*abs(param.Tx2());
            const sfloat_v dx2_est2 = Pick2*(abs(param.Err2X2() + hit.Err2X2()));
            inRange = (dx2*dx2 <= dx2_est2) || (dx2 > 0);  
          }
          else { // don't use sort for 3th station
            inRange = active;
          }
#else
          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);  
#endif // PANDA_STT
          
          if ( (!valid1 || (active && !inRange)).isFull() ) continue;
  
          iStartS2 = iS2*sfloat_v::Size + iV;
          firstFound = true;
            // break; // doesn't work
        } // iV
        iStartS2E = 0;
      } // iS2
#endif 
    
      iStartS2V = iStartS2/sfloat_v::Size;
      iStartS2E = iStartS2%sfloat_v::Size;
      for( unsigned int iS2 = iStartS2V; iS2 < b.size(); ++iS2 ) {
        const ITSCANPletV& s2 = b[iS2];
        const sfloat_m valid2 = s2.IsValid() && static_cast<sfloat_m>(ushort_v(Vc::IndexesFromZero) >= iStartS2E);
	//cout << valid2 << endl;
        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();
#if defined( PANDA_STT ) || defined( PANDA_FTS )
          float hgx, hgy, hgz;
          AliHLTTPCCAParameters::CALocalToGlobal( hit.X0(), hit.X1(), hit.X2(), hit.Angle(), hgx, hgy, hgz );
          sfloat_v pgx, pgy, pgz;
          AliHLTTPCCAParameters::CALocalToGlobal( param.X0(), param.X1(), param.X2(), param.Angle(), pgx, pgy, pgz );
          const sfloat_v dx = hgx - pgx;
          const sfloat_v dy = hgy - pgy;
#ifdef PANDA_STT
          active &=
            ( static_cast<sfloat_m>( (hit.IStation() >= AliHLTTPCCAParameters::NMVDStations && hit.IStation() < 8+AliHLTTPCCAParameters::NMVDStations)  || hit.IStation() > 16+AliHLTTPCCAParameters::NMVDStations ) &&
              ( dx*dx + dy*dy < 16.f ) ) || // hits are local. max one hit is missing
            ( dx*dx + dy*dy < 49.f ); // projection of stereo layer is about 7 cm
#else // PANDA_FTS
          // hits are not local, so skip it
          // int iSubLayer = hit.IStation()%8;
          // active &=
          //   ( static_cast<sfloat_m>( (iSubLayer == 1) || (iSubLayer == 7) ) && // current and previous are adjacent axial layers
          //     ( dx*dx + dy*dy < 16.f ) ) || // hits are local. max one hit is missing
          //   ( static_cast<sfloat_m>( (iSubLayer != 0) ) &&
          //     ( dx*dx + dy*dy < 121.f ) ) || // projection of stereo layer is about 10.5 cm
          //   static_cast<sfloat_m>( (iSubLayer == 0) );
#endif
          if ( active.isEmpty() ) continue;
#endif
          active &= param.Transport( hit, GetParameters(), active );

#ifdef USE_HITSSORT 
          const sfloat_v Pick2 = fPick*fPick;
#if defined( PANDA_STT ) || defined( PANDA_FTS )
          sfloat_m inRange;
          if (  hit.IStation() > AliHLTTPCCAParameters::NMVDStations ) {
            const sfloat_v dx1 = hit.X1Corrected(param) - param.X1() - 2*fMaxDX0*abs(param.Tx1());
            const sfloat_v dx1_est2 = Pick2*(abs(param.Err2X1() + hit.Err2X1()));
            inRange = ( (dx1*dx1 <= dx1_est2) || (dx1 < 0) );
          }
          else if (  hit.IStation() < AliHLTTPCCAParameters::NMVDStations ) {
            const sfloat_v dx2 = hit.X2() - param.X2() - 2*fMaxDX0*abs(param.Tx2());
            const sfloat_v dx2_est2 = Pick2*(abs(param.Err2X2() + hit.Err2X2()));
            inRange = ( (dx2*dx2 <= dx2_est2) || (dx2 < 0) );
          }
          else {
            inRange = active;
          }
#else
          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) );
#endif // PANDA_STT
          if ( (!valid1 || (active && !inRange)).isFull() ) { break; }
          
#endif
       
          active &= IsEqual( param, hit );
          if ( active.isEmpty() ) continue;

          if (fFindIter == 0)
          {
            active &= param.Filter( hit, GetParameters(), active, TRIPLET_CHI2_CUT );
            if ( active.isEmpty() ) continue;
          }
          else {
            active &= param.Filter( hit, GetParameters(), active );
            ITSCANPletV trip = ITSCANPletV( s1, s2, iV, param, active );
            active &= Refit( trip, *a.HitsRef() );
            active &= trip.Param().Chi2() < TRIPLET_CHI2_CUT;
            if ( active.isEmpty() ) continue;
            param = trip.Param();
          }
          
          active &= param.Transport( short(hit.IStation() + 1), GetParameters(), active );
          r.push_back( ITSCANPletV( s1, s2, iV, param, active ) );

          // foreach_bit( unsigned int iV1, active ) {
            const int pos =  (r.size()-1)*sfloat_v::Size + iV1;
            if (r.FirstElementIByHit0()[s1.IHit(0)[iV1].e] == -1)
              r.FirstElementIByHit0()[s1.IHit(0)[iV1].e] = pos;
            r.FirstElementIByHit0()[s1.IHit(0)[iV1].e + 1] = pos+1; // save the end
          // }
        } // iV
        iStartS2E = 0;
      } // iS2
      } // iV1
    } // iS1
  } // doublets
  
  return r;
}


sfloat_m AliHLTTPCCAGBTracker::Refit( ITSCANPletV& triplet, const ITSCAHits& hits )
{ 
  //* Fitting of the triplet by using Kalman Filter.
  
  // TODO Start from hit
  const int N = triplet.N();

  AliHLTTPCCATrackParamVector &param = triplet.Param();

  vector<ITSCAHitV> thits( N );

  sfloat_m active = triplet.IsValid();
  for ( unsigned short ihit = 0; ihit < N; ihit++ ) {
    const TESV& index = triplet.IHit(ihit);
 
    ITSCAHit hs[sfloat_v::Size];
    foreach_bit(unsigned short iV, active) {
      hs[iV] = hits[index.s[iV]][index.e[iV]];
    }
    thits[ihit] = ITSCAHitV( hs, active );
  }
    
  const ITSCAHitV& hit0 = thits[0];

//  const sfloat_v qpt = param.QPt();
//  const sfloat_v sinPhi = param.GetSinPhi();
//  const sfloat_v signCosPhi = param.GetSignCosPhi();
//  const sfloat_v dzDs = param.GetDzDs();
  ITSCATarget target = fTarget; 

 // target.SetErrQMom(20/3.f);
  // param.InitByTarget(target);
//  param.SetQPt(qpt);
//   // param.SetSinPhi(sinPhi);
//   // param.SetSignCosPhi( signCosPhi );
//   param.InitDirection( hit0.X0(), hit0.X1(), hit0.X2() );
// //  param.SetDzDs( dzDs );
  // param.SetAngle( hit0.Angle() );

  param.Transport( hit0, GetParameters(), active );
  param.InitCovMatrix(target.Err2QMom());

    
  for ( unsigned short ihit = 0; ihit < N; ihit++ ) {
    const ITSCAHitV& hit = thits[ihit];
    active &= param.Transport( hit, GetParameters(), active );
    active &= param.Filter( hit, GetParameters(), active );
  }
  return active;
}


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 ITSCANPlets& triplets,
                         unsigned int& nCalls)
{
  //* The minimal value of chi-square and maximum value of NDF for each track-candidate 
  //* are selection criteria of the best track candidate.
  
#ifdef DRIFT_TUBES
//  if (nCalls > 100) return; // avoid long processing in confusing cases
#endif
  nCalls++;
  
  const ITSCAElementsOnStation<ITSCANPlet>& trs = triplets.OnStation( ista );
  const ITSCANPlet* curr_trip = &(trs[currITrip]);
  
  if (curr_trip->Level() == 0){ // the end of the track -> check and store
    
      // -- finish with current track

//    if( cT.Level() < min_best_l - 1 ) return; // suppose that only one hit can be added by extender
//    cT.Fit( *trs.HitsRef(), fTarget, GetParameters() ); // fit to determine Chi2 and NDF // takes 20 times more time then the rest

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

  } else
  { // level != 0

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

        // check new triplet
      bool isUsed = false;
      const int newTripLength = new_trip->N();
      ASSERT( newTripLength - curr_trip->N() >= -1, newTripLength << " - " << curr_trip->N() );
      for( int i = curr_trip->N() - 1; i < newTripLength; i++ ) {
        if( triplets.OnStation( new_trip->ISta(0) ).GetHit( i, newITrip ).IsUsed() ) {
          isUsed = true;
          break;
        }
      }

      if (isUsed) {
          // cT.Fit( *trs.HitsRef(), fTarget, GetParameters() ); // fit to determine Chi2 and NDF
          
          //  no used hits allowed -> compare and store track
        if ( (cT.NDF() > bT.NDF()) || ( (cT.NDF() == bT.NDF()) && (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 triplet
        nT.Level()++;
        for( int i = curr_trip->N() - 1; i < newTripLength; i++ ) {
          nT.AddHit( new_trip->IHit(i) );
        }
        

            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();
        dqp = dqp/Cqp;
        nT.Chi2() += dqp*dqp;

	
#ifndef SAVE_ALL_CANDIDATES_DBG
    //    if( nT.Chi2() > TRACK_CHI2_CUT * nT.NHits() ) continue;
#endif
        
        FindBestCandidate(new_trip->ISta(0), bT, newITrip, nT, min_best_l, triplets, nCalls);
      } // add triplet to track
    } // for neighbours
  } // level = 0
}

sfloat_m AliHLTTPCCAGBTracker::IsEqual(
  const AliHLTTPCCATrackParamVector& param, const ITSCAHit& hit )
{
  //* Comparison of parameters of reconstructed and MC tracks. 
  //* The function returns TRUE, if the difference of parameters is less than minimal error.
  
  sfloat_m r = sfloat_m(true);

#ifndef DRIFT_TUBES
  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;
#else
  UNUSED_PARAM2(param,hit);
#endif
  
  return r;
}