//-*- Mode: C++ -*-
  // *****************************************************************************
  //                                                                             *
  // @Autors: I.Kulakov; M.Zyzak; I.Kisel                                        *
  // @e-mail: I.Kulakov@gsi.de; M.Zyzak@gsi.de; I.Kisel@compeng.uni-frankfurt.de *
  //                                                                             *
  // *****************************************************************************

#include "CAFunctionality.h"
#include "ITSCAHitsV.h"
#include "ITSCATarget.h"

sfloat_m CAFunctionality::FitIteration(const AliHLTTPCCAParam& caParam, const ITSCAHits& hits, AliHLTTPCCATrackParamVector& param, const vector<TESV>& iHits, const ITSCATarget& target, bool dir, bool useParam, const sfloat_m& mask)
{
  const int NTHits = iHits.size();
  sfloat_m active = mask;
  
    // get hits
  vector<ITSCAHitV> thits( NTHits );
  for ( unsigned short ihit = 0; ihit < NTHits; ihit++ ) {
    TESV index = iHits[ihit];
    if (!dir) index = iHits[NTHits-1-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];

  if (!useParam) {
    sfloat_v qMom = param.QMomentum();
    param.InitByTarget(target);
    param.SetQMomentum(qMom);
    param.InitDirection( hit0.X0(), hit0.X1(), hit0.X2() );
    param.SetAngle( hit0.Angle() );
  }
  else {
    param.InitCovMatrix( target.Err2QMom() );
  }
    
  for ( unsigned short ihit = 0; ihit < NTHits; ihit++ ) {
    const ITSCAHitV& hit = thits[ihit];
    active &= param.Transport( hit, caParam, active );
    active &= param.Filter( hit, caParam, active );
  }

  return active;
}

sfloat_m CAFunctionality::Fit(const AliHLTTPCCAParam& caParam, const ITSCAHits& hits, AliHLTTPCCATrackParamVector& param, const vector<TESV>& iHits, const ITSCATarget& target, bool dir, const sfloat_m& mask)
{
  sfloat_m active = mask;
  int i = 0;

  sfloat_v qMom = sfloat_v(10e10f);

#if 1
  while ( (++i < 10) && ( abs( (param.QMomentum() - qMom)/qMom ) > 0.005f ) ) {
    qMom = param.QMomentum();
    active &= FitIteration( caParam, hits, param, iHits, target, dir, false, active );
  }
#else // for Panda it is and slower, since doesn't use target z-coor?
  active &= FitIteration( caParam, hits, param, iHits, target, dir, false, active );
  while ( (++i < 5) && ( abs( (param.QMomentum() - qMom)/qMom ) > 0.005f ) ) {
    qMom = param.QMomentum();
    active &= FitIteration( caParam, hits, param, iHits, target, !dir, true, active );
    active &= FitIteration( caParam, hits, param, iHits, target, dir, true, active );
  }
#endif
  return active;
}