/***************************************************************************
                        DataFT.cxx  -  description
  Class encapsulates the track measurement - coordinates measured in each
  plane; used by the least squares track fitter.
                             -------------------
    begin                : Thu Jun 26 2003
    author               : Sergei Avvakumov
    email                : avva@dunk
 ***************************************************************************/
#include <vector>
#include <algorithm>
#include <cassert>
#include <cmath>

#include "TVectorD.h"
#include "TGraph.h"
#include "TSpline.h"

#include "DataUtil/CDL2STL.h"
#include "MessageService/MsgService.h"
#include "MessageService/MsgFormat.h"
#include "CandTrackSR/TrackClusterSR.h"
#include "RecoBase/CandStripHandle.h"
#include "RecoBase/CandTrackHandle.h"
#include "Plex/PlexPlaneId.h"
#include "Swimmer/SwimSwimmer.h"
#include "Swimmer/SwimParticle.h"
#include "Swimmer/SwimZCondition.h"

#include "GeoSwimmer/GeoSwimmer.h"
#include "GeoSwimmer/GeoSwimParticle.h"
#include "GeoSwimmer/GeoSwimZCondition.h"

#include "CandFitTrackSA/ConstFT.h"
#include "CandFitTrackSA/DataFT.h"
#include "CandFitTrackSA/TracerSA.h"
#include "CandFitTrackSA/TrackContext.h"

using namespace ConstFT;

CVSID("$Id: DataFT.cxx,v 1.20 2008/03/04 00:42:07 ishi Exp $");

///
///
///
// DataFT::DataFT() :
//     fNPlanesUsed(0), fEMCharge(0), fDir(0),
//     fInitialTrack(NTrackParams), fZVtx(0.),
//     fPlanes(NPlanesMax)
// {
//     TracerSA trace("DataFT::DataFT()");
// }

///
///
///
DataFT::DataFT(const AlgConfig& ac, const TrackContext& tc) :
    fNPlanesUsed(tc.GetNPlanes()), fDir(tc.GetDir()),
    fInitialTrack(NTrackParams), fZVtx(0.),
    fPlanes(tc.GetNPlanes()),
    fUHitUse(tc.GetNPlanes()), fVHitUse(tc.GetNPlanes()),
    fGeo(tc), fMinStripCharge(kMinStripCharge),
    fSetLPos(kTRUE), fNHitsInViewMin(4),
    fUseGeoSwimmer(false)
{
    TracerSA trace("DataFT::DataFT(const AlgConfig& , const TrackContext& )");

    Config(ac);

    Bool_t status;

    status = Init(tc);
    assert(status && "DataFT::Init(const TrackContext&) failed!");

    status = Fill(tc);
    assert(status && "DataFT::Fill(const TrackContext&) failed!");

    if ( GetNUHits() >= fNHitsInViewMin &&
            GetNVHits() >= fNHitsInViewMin ) {
        status = FillLinWithSpline();
        assert(status && "DataFT::FillLinWithSpline() failed!");
    }
    
    PrintData();
}


///
///
///
DataFT::~DataFT()
{
    TracerSA trace("DataFT::~DataFT()");
}


///
///
///
void DataFT::Reset()
{
    TracerSA trace("DataFT::Reset()");
    fNPlanesUsed = 0;
    fDir = 0;
    fInitialTrack.Zero();
    fZVtx = 0.;
    fPlanes.clear();
}


///
/// read configuration parameters from AlgConfig
///
void DataFT::Config(const AlgConfig& ac)
{
    TracerSA trace("DataFT::Config(const AlgConfig&)");

    if ( ac.KeyExists("NHitsInViewMin") ) {
        fNHitsInViewMin = ac.GetInt("NHitsInViewMin");
    }

    if ( ac.KeyExists("MinStripCharge") ) {
        fMinStripCharge = ac.GetDouble("MinStripCharge");
    }

    if ( ac.KeyExists("SetLPos") ) {
        fSetLPos = (Bool_t) ac.GetInt("SetLPos");
    }

    if ( ac.KeyExists("UseGeoSwimmer") ) {
      fUseGeoSwimmer = ac.GetInt("UseGeoSwimmer");
    }

}


///
///
///
Bool_t DataFT::Init(const TrackContext& tc)
{
    TracerSA trace("DataFT::Init(const TrackContext& )");

    Int_t begin = tc.GetBegPlane();
    Int_t end   = tc.GetEndPlane();

    //
    fNPlanesUsed = abs(begin - end) + 1;
    fDir = end > begin ? 1 : -1;
    fInitialTrack.Zero();

    // fill plane information - Z positions, thickness, X0
    fPlanes.resize(fNPlanesUsed);
    Int_t plane = begin;
    VldContext vldc = tc.GetVldContext();

    if(fUseGeoSwimmer) {
      GeoSwimmer::Instance()->Initialize(vldc);      
    }

    for (PlaneDataItr it = fPlanes.begin(); it != fPlanes.end(); ++it) {
        PlexPlaneId planeid = PlexPlaneId(vldc.GetDetector(), plane);
        it->plane = planeid;
        it->z = fGeo.GetZ(planeid);
        it->dz = fGeo.GetdZSteel(planeid);
        it->x0 = fGeo.GetX0(planeid);
        plane += fDir;
    }

    // set vertex z position - either in the middle of 
    // the "previous" steel plane or just 0.03m back
    // from the begin plane
    fZVtx = fGeo.GetZVtx(PlexPlaneId(vldc.GetDetector(), begin), fDir);
    
    return kTRUE;
}


// following methods used these typedefs (defined in .h)
// typedef const CandStripHandle CSH;
// typedef CSH* CSHPtr;
// typedef std::vector<CSHPtr>::iterator CSHItr;
// typedef std::multimap<Int_t, CSHPtr> StripMap;
// typedef StripMap::iterator StripMapItr;

///
///
///
Bool_t DataFT::Fill(const TrackContext& tc)
{
  TracerSA trace("DataFT::Fill(const TrackContext&)");

  // convert daughter list to STL vector
  //vector<CSHPtr> stripList(DataUtil::CDL2STLvector<CSH>(*tc.GetTrackHandle()));

  Reco::StripVec_t stripList = tc.GetStripVec();
  
  // sort using ByPlane functor    
  sort(stripList.begin(), stripList.end(), Reco::ByPlane());
  // reverse order if track direction against Z axis
  if (fDir<0) reverse(stripList.begin(), stripList.end());

  // strip _multi_map - can hold more than one strip per plane 
  StripMap stripMap;

  // Fill stripMap
  for ( Reco::StripIter_t it = stripList.begin();  it != stripList.end(); ++it) {

      MSG("FitTrackSA", Msg::kVerbose)
      << "P " << (*it)->GetPlane()
      << ": Charge=" << (*it)->GetCharge()
      << "; Strip=" << (*it)->GetStrip() << "\n";

      // check if strip is not demux vetoed and charge is not too small
      if ( (*it)->GetDemuxVetoFlag() ) continue;
      if ( (*it)->GetCharge() < fMinStripCharge ) continue;

      // And add it to the multimap
      Int_t plane = (*it)->GetPlane();
      stripMap.insert(pair<Int_t, Reco::Strip_t> (plane, *it));
  }

  // Loop over planes, use only planes with one or two (consecutive) hit strips
  // ignore planes with more than two hit strips - let shower code deal with them
  Int_t begplane = GetBegPlaneId().GetPlane();
  Int_t nplanes  = GetNPlanes();
  for (Int_t i = 0; i < nplanes; ++i) {
      Int_t plane = begplane + i*fDir;
      switch ( stripMap.count(plane) ) {
      case 1:
          {
          StripMapItr itr = stripMap.find(plane);
          OneStripPlane(i, itr, tc.GetTrackHandle());
          }
          break;

      case 2:
          {
          StripMapItr itLower = stripMap.lower_bound(plane);
          StripMapItr itUpper = stripMap.upper_bound(plane);
          TwoStripPlane(i, itLower, itUpper, tc.GetTrackHandle());
          }
          break;

      default:
          // do nothing
          assert(1);
      }
  }
  return kTRUE;
}


///
/// set measured coordinates
///
void DataFT::SetHitCoords(  Count_t planeIndex,
                            PlaneData::Data_t tpos,
                            PlaneData::Data_t error )
{
    TracerSA trace("DataFT::SetHitCoords(Int_t, PlaneData::Data_t, PlaneData::Data_t)");
    PlexPlaneId id = GetPlaneId(planeIndex);
    if ( id.GetPlaneView() == PlaneView::kU ) {
        SetUHit(planeIndex);
        SetUHitUse(planeIndex);
        SetU(planeIndex, tpos);
        SetSigmaU(planeIndex, error);

        UnsetVHit(planeIndex);
        UnsetVHitUse(planeIndex);
        SetV(planeIndex, 0.0);
        SetSigmaV(planeIndex, 0.0);
    } else if ( id.GetPlaneView() == PlaneView::kV ) {
        SetVHit(planeIndex);
        SetVHitUse(planeIndex);
        SetV(planeIndex, tpos);
        SetSigmaV(planeIndex, error);

        UnsetUHit(planeIndex);
        UnsetUHitUse(planeIndex);
        SetU(planeIndex, 0.0);
        SetSigmaU(planeIndex, 0.0);
    }
}

///
/// set measured coord for a plane with a single strip hit
///
void DataFT::OneStripPlane(Count_t planeIndex, StripMapItr it,
                                                    const CandTrackHandle* cth)
{
    TracerSA trace("DataFT::OneStripPlane");
    PlaneData::Data_t tpos(0.);
    if ( fSetLPos ) {
        tpos = fGeo.GetRotationCorrectedTPos(it->second, cth);
    } else {
        tpos = it->second->GetTPos();
    }

    PlaneData::Data_t tposrms = kOneStripError;

    SetHitCoords(planeIndex, tpos, tposrms);
}


///
/// set measured coord for a plane with two strips hit
///
void DataFT::TwoStripPlane(Count_t planeIndex,
                            StripMapItr lower, StripMapItr upper,
                            const CandTrackHandle* cth)
{
    TracerSA trace("DataFT::TwoStripPlane");

  // running sums
  PlaneData::Data_t chargesum(0.);
  PlaneData::Data_t tpossum(0.);
  PlaneData::Data_t tposrmssum(0.);
  Int_t nstrip(0);

  // loop over strips
  for (StripMapItr it = lower; it != upper; ++it) {
    // cache strip charge, tpos
    PlaneData::Data_t charge = it->second->GetCharge();
    // make sure strips with negative charge have been
    // cleaned
    assert( charge > 0. && "Strip charge < 0.!!!!");

    PlaneData::Data_t tpos(0.);
    if ( fSetLPos ) {
        tpos = fGeo.GetRotationCorrectedTPos(it->second, cth);
    } else {
        tpos = it->second->GetTPos();
    }

    // increment running sums
    chargesum  += charge;
    tpossum    += tpos;
    tposrmssum += tpos*tpos;
    ++nstrip;
  }

  // calculate charge weighted position, rms
  // of the hit
  PlaneData::Data_t tpos = tpossum/nstrip;

  // initialize rms to that of a songle strip measurement
  PlaneData::Data_t tposrms = kOneStripError;

  PlaneData::Data_t tposrms2 = tposrmssum/nstrip - tpos*tpos;
  // ignore zero values, they only happen if the same strip is used twice
  // (this happens in cedar mc)
  if (tposrms2 > 0.) {
    tposrms = sqrt(tposrms2);
  }

  SetHitCoords(planeIndex, tpos, tposrms);
}


///
///
///
void DataFT::SetNPlanesUsed(Count_t n)
{
    TracerSA trace("DataFT::SetNPlanesUsed");
    assert( n <= GetNPlanes() && "# of used planes must not be greater "
                                "than the total # of planes!");
    fNPlanesUsed = n;
}


///
///
///
Int_t DataFT::GetEMCharge() const
{
    return (Int_t) TMath::Sign(1., fInitialTrack(kQoverP));
}


///
///
///
PlaneData::Data_t
DataFT::GetUMean() const
{
    if ( fNPlanesUsed > 0 ) {
        return (GetUf(0) + GetUf(fNPlanesUsed-1))/2.;
    } else {
        return GetUf(0);
    }
}


///
///
///
PlaneData::Data_t
DataFT::GetVMean() const
{
    if ( fNPlanesUsed > 0 ) {
        return (GetVf(0) + GetVf(fNPlanesUsed-1))/2.;
    } else {
        return GetVf(0);
    }
}


///
///
///
DataFT::Count_t DataFT::GetNUPlanes() const
{
    TracerSA trace("DataFT::GetNUPlanes");
    int nplanes=0;

    PlaneDataCItr beg = fPlanes.begin();
    PlaneDataCItr end = fPlanes.end();

    for (PlaneDataCItr it = beg; it != end; ++it) {
        if ( it->plane.GetPlaneView() == PlaneView::kU ) ++nplanes;
    }
    return nplanes;
}


///
///
///
DataFT::Count_t DataFT::GetNVPlanes() const
{
    TracerSA trace("DataFT::GetNVPlanes");
    int nplanes=0;

    PlaneDataCItr beg = fPlanes.begin();
    PlaneDataCItr end = fPlanes.end();

    for (PlaneDataCItr it = beg; it != end; ++it) {
        if ( it->plane.GetPlaneView() == PlaneView::kV ) ++nplanes;
    }
    return nplanes;
}


///
///
///
DataFT::Count_t DataFT::GetNUHits() const
{
    TracerSA trace("DataFT::GetNUHits");
    int nhits=0;
    int size = GetNPlanes();

    for (int i = 0; i < size; ++i) {
        if ( IsHitU(i) ) ++nhits;
    }
    return nhits;
}


///
///
///
DataFT::Count_t DataFT::GetNVHits() const
{
    TracerSA trace("DataFT::GetNVHits");
    int nhits=0;
    int size = GetNPlanes();

    for (int i = 0; i < size; ++i) {
        if ( IsHitV(i) ) ++nhits;
    }
    return nhits;
}


///
///
///
DataFT::Count_t DataFT::GetNHits() const
{
    TracerSA trace("DataFT::GetNHits");
    int nhits=0;
    int size = GetNPlanes();

    for (int i = 0; i < size; ++i) {
        if ( IsHitU(i) ) ++nhits;
        if ( IsHitV(i) ) ++nhits;
    }
    return nhits;
}


///
///
///
DataFT::Count_t DataFT::GetNUHitsUsed() const
{
    TracerSA trace("DataFT::GetNUHitsUsed");
    int nhits=0;
    int size = GetNPlanesUsed();
    for (int i = 0; i < size; ++i) {
        if ( UHitUse(i) ) ++nhits;
    }
    return nhits;
}


///
///
///
DataFT::Count_t DataFT::GetNVHitsUsed() const
{
    TracerSA trace("DataFT::GetNVHitsUsed");
    int nhits=0;
    int size = GetNPlanesUsed();
    for (int i = 0; i < size; ++i) {
        if ( VHitUse(i) ) ++nhits;
    }
    return nhits;
}

///
///
///
DataFT::Count_t DataFT::GetNPlanesMin(Count_t nhitsperview) const
{
    TracerSA trace("DataFT::GetNPlanesMin");
    int nplanesmin=0;
    int nuhits=0;
    int nvhits=0;

    int size = GetNPlanes();
    for (int i = 0; i != size; ++i) {
        ++nplanesmin;
        if ( UHitUse(i) ) ++nuhits;
        if ( VHitUse(i) ) ++nvhits;

        if ( nuhits >= nhitsperview && nvhits >= nhitsperview )
                                                        return nplanesmin;
    }

    assert(0 && "Not enough planes in view!");
    return 0;
}


///
///
///
DataFT::Count_t DataFT::GetNHitsUsed() const
{
    TracerSA trace("DataFT::GetNHitsUsed");
    int nhits=0;
    int size = GetNPlanesUsed();
    for (int i = 0; i < size; ++i) {
        if ( UHitUse(i) ) ++nhits;
        if ( VHitUse(i) ) ++nhits;
    }
    return nhits;
}


///
///
///
DataFT::Count_t DataFT::GetBegHit() const
{
    TracerSA trace("DataFT::GetBegHit");
    int size = GetNPlanesUsed();
    for (int i = 0; i < size; ++i) {
        if ( IsHitU(i) || IsHitV(i) ) return GetPlane(i);
    }
    return 0;
}


///
///
///
DataFT::Count_t  DataFT::GetEndHit() const
{
    TracerSA trace("DataFT::GetEndHit");
    int size = GetNPlanesUsed();
    for (int i = 0; i < size; ++i) {
        if ( IsHitU(size-1-i) || IsHitV(size-1-i) ) 
            return GetPlane(size-1-i);
    }
    return 0;
}



///
///
///
DataFT::Count_t DataFT::GetBegHitUsed() const
{
    TracerSA trace("DataFT::GetBegHit");
    int size = GetNPlanesUsed();
    for (int i = 0; i < size; ++i) {
        if ( UHitUse(i) || VHitUse(i) ) return GetPlane(i);
    }
    return 0;
}


///
///
///
DataFT::Count_t DataFT::GetEndHitUsed() const
{
    TracerSA trace("DataFT::GetEndHit");
    int size = GetNPlanesUsed();
    for (int i = 0; i < size; ++i) {
        if ( UHitUse(size-1-i) || VHitUse(size-1-i) ) 
            return GetPlane(size-1-i);
    }
    return 0;
}



///
///
///
void DataFT::PrintData() const
{
    TracerSA trace("DataFT::PrintData");
    MsgStream *mftsa = &MSGSTREAM("FitTrackSA", Msg::kVerbose);
    (*mftsa) << "Data:\n";
    MsgFormat ffmt("%7.3f");
    
    for (PlaneDataCItr it = fPlanes.begin(); it != fPlanes.end(); ++it) {
        (*mftsa) << it->plane 
        << ": U=" << ffmt(it->u) << "; EU=" << ffmt(it->eu) 
        << "; UL=" << ffmt(it->ulin) << "; dU/dZL=" << ffmt(it->dudzlin) 
        << "; V=" << ffmt(it->v) << "; EV=" << ffmt(it->ev) 
        << "; VL=" << ffmt(it->vlin) << "; dV/dZL=" << ffmt(it->dvdzlin)
        << "; Z=" << ffmt(it->z) << "; X0="   << ffmt(it->x0) << std::endl;
    }
    return;
}


///
///
///
void DataFT::FillVectorRes(TMatrixD& vC, PlaneData::Data_t cdata, PlaneData::Data_t ctrack) const
{
    TracerSA trace("DataFT::FillVectorRes");
    vC.ResizeTo(GetNHitsUsed(),1);
    Int_t ihits = 0;
    for (Int_t i=0; i<GetNPlanesUsed(); ++i) {
        if ( UHitUse(i) ) {
            vC(ihits,0) = cdata*GetU(i) + ctrack*GetUf(i);
            ++ihits;
        }
    }
    for (Int_t i=0; i<GetNPlanesUsed(); ++i) {
        if ( VHitUse(i) ) {
            vC(ihits,0) = cdata*GetV(i) + ctrack*GetVf(i);
            ++ihits;
        }
    }
    return;
}


///
///
///
void DataFT::FillVectorC(TMatrixD& vC) const
{
    TracerSA trace("DataFT::FillVectorC");
    FillVectorRes(vC, 1., 0.);
}


///
///
///
void DataFT::FillVectorCF(TMatrixD& vC) const
{
    TracerSA trace("DataFT::FillVectorCF");
    FillVectorRes(vC, 0., 1.);
}


///
///
///
Bool_t DataFT::FillUArrays(Double_t zu[], Double_t u[], Int_t n)
{
    TracerSA trace("DataFT::FillUArrays(Double_t zu[], Double_t u[],Int_t)");
    
    Int_t index = 0;    
    Count_t size = GetNPlanes();
    for (Count_t i = 0; i < size; ++i) {
        if ( IsHitU(i) ) {
            assert( index < n && "Array bound check failed!");
            zu[index] = (Double_t) GetZ(i);
            u[index] = (Double_t) GetU(i);
            ++index;
        }
    }
    
    MsgStream *mftsa = &MSGSTREAM("FitTrackSA", Msg::kVerbose);
    (*mftsa) << "Filling UArrays: " << "\n";
    MsgFormat ffmt("%7.3f");
    // fill spline approximation of the track
    for (Count_t i = 0; i<n; ++i) {
        (*mftsa) << "z=" << ffmt(zu[i])
                 << "; u=" << ffmt(u[i]) << std::endl;
    }
    return kTRUE;
}


///
///
///
Bool_t DataFT::FillVArrays(Double_t zv[], Double_t v[], Int_t n)
{
    TracerSA trace("DataFT::FillVArrays(Double_t zv[], Double_t v[],Int_t)");
    
    Int_t index = 0;
    int size = GetNPlanes();
    for (int i = 0; i < size; ++i) {
        if ( IsHitV(i) ) {
            assert( index < n && "Array bound check failed!");
            zv[index] = (Double_t) GetZ(i);
            v[index] = (Double_t) GetV(i);
            ++index;
        }
    }    
    
    MsgStream *mftsa = &MSGSTREAM("FitTrackSA", Msg::kVerbose);
    (*mftsa) << "Filling VArrays: " << "\n";
    MsgFormat ffmt("%7.3f");
    // fill spline approximation of the track
    for (Count_t i = 0; i<n; ++i) {
        (*mftsa) << "z=" << ffmt(zv[i]) << "; v=" << ffmt(v[i]) << std::endl;
    }
    return kTRUE;
}


///
///
///
Bool_t DataFT::FillLinWithSpline()
{
    TracerSA trace("DataFT::FillLinWithSpline()");

    MsgStream *mftsa = &MSGSTREAM("FitTrackSA", Msg::kVerbose);

    const Int_t NU = GetNUHits();
    std::vector<Double_t> zu(NU);
    std::vector<Double_t> u(NU);
    FillUArrays(&zu[0], &u[0], NU);
    (*mftsa) << "NU = " << NU << std::endl;
    TSpline5 splineU("U", &zu[0], &u[0], NU);

    const Int_t NV = GetNVHits();
    std::vector<Double_t> zv(NV);
    std::vector<Double_t> v(NV);
    FillVArrays(&zv[0], &v[0], NV);
    (*mftsa) << "NV = " << NV << std::endl;
    TSpline5 splineV("V", &zv[0], &v[0], NV);

    (*mftsa) << "Filling from TSpline: " << "\n";
    MsgFormat ffmt("%7.3f");
    // fill spline approximation of the track
    for (Count_t i = 0; i<GetNPlanes(); ++i) {
        Double_t z = (Double_t) GetZ(i);

        (*mftsa) << "z=" << ffmt(z)
            << "; US=" << ffmt(splineU.Eval(z))
            << "; dU/dZS=" << ffmt(splineU.Derivative(z))
            << "; VS=" << ffmt(splineV.Eval(z))
            << "; dV/dZS=" << ffmt(splineV.Derivative(z))
            << std::endl;

        SetUlin(i, (PlaneData::Data_t) splineU.Eval(z));
        SetDudzlin(i, (PlaneData::Data_t) splineU.Derivative(z));
        SetVlin(i, (PlaneData::Data_t) splineV.Eval(z));
        SetDvdzlin(i, (PlaneData::Data_t) splineV.Derivative(z));
    }
    return kTRUE;
}


///
///
///
DataFT::Count_t DataFT::GetNPlanesPCut(PlaneData::Data_t pcut) const
{
    TracerSA trace("DataFT::GetNPlanesPCut");
    // return number of planes with p>pcut
    PlaneData::Data_t invpcut = 1./pcut;
    
    //CRItr itlastused = fPlanes.rend()-fNPlanesUsed;
    for (PlaneDataCRItr it = fPlanes.rend()-fNPlanesUsed; 
                                                it != fPlanes.rend(); ++it) {
        if ( fabs(it->invp) < invpcut ) return fPlanes.rend()-it; 
    }
    
    return 0;
}


///
///
///
DataFT::Count_t DataFT::SwimAsSwimmer(Count_t nplanesuse, SwimSwimmer& swimmer)
{
    TracerSA trace("DataFT::SwimAsSwimmer");
    
    MSG("FitTrackSA",Msg::kVerbose) << "Request to swim " 
                                    << nplanesuse << " planes.\n"; 

    // Swim track starting from the first plane, initial values 
    // from fInitialTrack. Swim through nplanesuse planes.
    
    // Check if number of planes to swim is reasonable
    if ( nplanesuse<1 ) {
        MSG("FitTrackSA",Msg::kWarning) 
            << "Request to swim less than one plane. Doing Nothing\n";
        return 0;
    } else if ( nplanesuse > GetNPlanes() ) {
        MSG("FitTrackSA",Msg::kWarning) 
        << "Request to swim more planes than we have.\n";
        nplanesuse = GetNPlanes();
    }

    // Initialize the swim particle from initial values of parameters
    
    // need these to get return values from xy2uv, uv2xy functions
    double x=0.;
    double y=0.;
    double u=0.;
    double v=0.;
    
    // convert track position (u, v) -> (x, y)
    fGeo.uv2xy(fInitialTrack(kU), fInitialTrack(kV), x, y);
    TVector3 position(x, y, fZVtx);
    
    // convert track direction
    PlaneData::Data_t pz, invpz;
    invpz = sqrt(1. + pow(fInitialTrack(kdUdZ),2) +
                      pow(fInitialTrack(kdVdZ),2)) * 
            TMath::Abs(fInitialTrack(kQoverP));
            
    if ( invpz < TinyNumber ) {
        MSG("FitTrackSA",Msg::kDebug) 
            << "1./Pz=" <<invpz<<" too small => replace with TinyNumber\n" ;
        invpz = TinyNumber;
    }
    
    pz = GetZdir()/invpz;
    fGeo.uv2xy(fInitialTrack(kdUdZ), fInitialTrack(kdVdZ), x, y);
    TVector3 momentum(pz*x, pz*y, pz);

    // create the swim particle
    SwimParticle muon(position,momentum);
    muon.SetCharge(GetEMCharge());

    int nplanesused = 0; // reset to 0
    MsgStream *mftsa = &MSGSTREAM("FitTrackSA", Msg::kVerbose);
    (*mftsa) << "Swimming track:\n";
    MsgFormat ffmt("%7.3f");
    // Loop over planes
    for (PlaneDataItr it = fPlanes.begin(); 
                                        it != fPlanes.begin()+nplanesuse; ++it) {
        // Swim condition -> to the current plane
        SwimZCondition zc(it->z);

        Bool_t status; // Swimmer status

	if(fUseGeoSwimmer) {
	  status = GeoSwimmer::Instance()->SwimForward(muon,it->z);
	} else {
	  status = swimmer.SwimForward(muon,zc);
	}

        if ( status ) {
 
            fGeo.xy2uv(muon.GetPosition().X(), muon.GetPosition().Y(), u, v);
            it->uf = u;
            it->vf = v;
            
            fGeo.xy2uv(muon.GetDirection().X(), muon.GetDirection().Y(), u, v);
            it->dudz = u/muon.GetDirection().Z();
            it->dvdz = v/muon.GetDirection().Z();
            
            it->invp = muon.GetCharge()/muon.GetMomentumModulus();
            it->cos = pow(1.+pow(it->dudz,2)+pow(it->dvdz,2), -0.5);
            
            it->s = muon.GetS();
            it->r = muon.GetRange();
            
                
            ++nplanesused;
            
            // swim succeded -> fill track parameters for the next plane
            (*mftsa) 
                << it->plane << ": z = " << ffmt(it->z) 
                << "; u = " << ffmt(it->uf) << "; v = " << ffmt(it->vf) 
                << "; dudz = " << ffmt(it->dudz)<<"; dvdz = " << ffmt(it->dvdz)
                << "; invp = " << ffmt(it->invp)<<"; cos = " << ffmt(it->cos)
                << "; s = " << ffmt(it->s) << "; r = " << ffmt(it->r) << "\n";
        } else {
            // swim failed -> print info and break out of the loop
            MSG("FitTrackSA",Msg::kDebug) 
                << "Swim failed at " << it->plane 
                << ", z = " << it->z << "\n";
            DumpTrack();
            // SetNPlanesUsed(it - fPlanes.begin());
            SetNPlanesUsed(nplanesused);
            return nplanesused;
        }
    }
    
    SetNPlanesUsed(nplanesused);
    return nplanesused;
}


///
///
///
DataFT::Count_t DataFT::SwimAsSwimmer(SwimSwimmer& swimmer)
{
    return SwimAsSwimmer(GetNPlanes(), swimmer);
}


///
///
///
PlaneData::Data_t
DataFT::ThetaMCS(Count_t i) const
{
    // Calculate MCS angle in the i-th plane
    return Theta0_P1*sqrt(GetX0(i)/GetCos(i))*
            (1.+Theta0_P2*log(GetX0(i)/GetCos(i)))*GetInvP(i);
}


///
///
///
PlaneData::Data_t
DataFT::T(Count_t i, Count_t n) const
{
    // Calculate T(i,n) - for the covarince matrix
    //   return ( (n-i)*dz + d )/fCos(i);
    return  TMath::Abs(GetZ(n)-GetZ(i)) ;
}


///
///
///
void DataFT::DumpTrack() const
{
    TracerSA trace("DataFT::DumpTrack");
    // Print out track (for debugging)
    if ( GetNPlanesUsed()>0 ) {
        MsgFormat ffmt("%7.3f");
        MsgStream *mftsa = &MSGSTREAM("FitTrackSA", Msg::kVerbose);
        (*mftsa) << "Generated track:\n";
        for (Int_t i = 0; i<GetNPlanesUsed(); i++) {
            (*mftsa)
                << GetPlane(i) <<  ": u=" << ffmt(GetUf(i))
                << "; du/dz=" << ffmt(GetDudz(i)) <<  "; v=" << ffmt(GetVf(i))
                << "; dv/dz=" << ffmt(GetDvdz(i)) 
                << "; p=" <<ffmt(1./TMath::Max(TinyNumber,(Double_t)GetInvP(i)))
                << "; z=" << ffmt(GetZ(i)) <<  "\n";
        }
    } else {
        MSG("FitTrackSA",Msg::kVerbose) << "No track generated yet!\n";
    }

}


///
///
///
void DataFT::PrintMomenta() const
{
    TracerSA trace("DataFT::PrintMomenta");
    MsgFormat ffmt("%7.3f");
    MsgStream *mftsa = &MSGSTREAM("FitTrackSA", Msg::kVerbose);
    (*mftsa) << "Generated track:\n";
    for (Int_t i = 0; i<GetNPlanesUsed(); i++) {
        (*mftsa) << GetPlane(i) 
            << ": p=" << ffmt(1./GetInvP(i)) <<  "\n";
    }

}


///
///
///
void DataFT::SetInitial(const TVectorD& inittrack)
{
    TracerSA trace("DataFT::SetInitial");
    //
    fInitialTrack = inittrack;
    return;
}

///
///
///
DataFT::Count_t DataFT::SetInitial(const TVectorD& inittrack, Count_t nplanesuse,
                                                    SwimSwimmer& swimmer)
{
    TracerSA trace("DataFT::SetInitial(const TVectorD&,Int_t,SwimSwimmer&)");
    //
    fInitialTrack = inittrack;
    
    return SwimAsSwimmer(nplanesuse, swimmer);
}