/home/bes3soft/bes3soft/Boss/7.0.2/dist/7.0.2/Reconstruction/TrkReco/TrkReco-00-08-59-patch4-slc6tag/src/TTrack.cxx

Go to the documentation of this file.

#define OPTNK
//-----------------------------------------------------------------------------
// $Id: TTrack.cxx,v 1.24.8.1 2012/10/29 03:46:45 xuqn Exp $
//-----------------------------------------------------------------------------
// Filename : TTrack.h
// Section  : Tracking
// Owner    : Yoshi Iwasaki
// Email    : yoshihito.iwasaki@kek.jp
//-----------------------------------------------------------------------------
// Description : A class to represent a track in tracking.
//               See http://bsunsrv1.kek.jp/~yiwasaki/tracking/
//-----------------------------------------------------------------------------

/* for isnan */
#if defined(__sparc)
#  if defined(__EXTENSIONS__)
#    include <cmath>
#  else
#    define __EXTENSIONS__
#    include <cmath>
#    undef __EXTENSIONS__
#  endif
#elif defined(__GNUC__)
#  if defined(_XOPEN_SOURCE)
#    include <cmath>
#  else
#    define _XOPEN_SOURCE
#    include <cmath>
#    undef _XOPEN_SOURCE
#  endif
#endif
#include <cfloat>
#define HEP_SHORT_NAMES
#include "CLHEP/String/Strings.h"
#include "CLHEP/Matrix/Vector.h"
#include "CLHEP/Matrix/SymMatrix.h"
#include "CLHEP/Matrix/DiagMatrix.h"
#include "CLHEP/Matrix/Matrix.h"
#include "TrkReco/TCircle.h"
#include "TrkReco/TMLine.h"
#include "TrkReco/TMLink.h"
#include "TrkReco/TMDCUtil.h"
#include "TrkReco/TMDCWire.h"
#include "TrkReco/TMDCWireHit.h"
#include "TrkReco/TMDCWireHitMC.h"
//#include "TrkReco/TMDCStrip.h"
#include "TrkReco/TTrack.h"
#include "TrkReco/TSegment.h"
//#include "tables/cdc.h"
//#include "tables/trk.h"
//#include "tables/mdst.h"
//#include "tables/hepevt.h"
#include "MdcTables/MdcTables.h"
#include "MdcTables/TrkTables.h"
#include "MdcTables/MdstTables.h"
#include "MdcTables/HepevtTables.h"

#include "CLHEP/Matrix/Matrix.h"
#include "GaudiKernel/StatusCode.h"
#include "GaudiKernel/IInterface.h"
#include "GaudiKernel/Kernel.h"
#include "GaudiKernel/Service.h"
#include "GaudiKernel/ISvcLocator.h"
#include "GaudiKernel/SvcFactory.h"
#include "GaudiKernel/IDataProviderSvc.h"
#include "GaudiKernel/Bootstrap.h"
#include "GaudiKernel/MsgStream.h"
#include "GaudiKernel/SmartDataPtr.h"
#include "GaudiKernel/IMessageSvc.h"



using CLHEP::HepVector;
using CLHEP::HepSymMatrix;
using CLHEP::HepDiagMatrix;
using CLHEP::HepMatrix;

const THelixFitter
TTrack::_fitter = THelixFitter("TTrack Default Helix Fitter");

TTrack::TTrack(const TCircle & c)
: TTrackBase(c.links()),
  _helix(new Helix(ORIGIN, Vector(5, 0), SymMatrix(5, 0))), 
  _charge(c.charge()),
  _ndf(0),
  _chi2(0.),
  _name("none"),
  _type(0),
  _state(0),
  _mother(0),
  _daughter(0) {

    //jialk
    StatusCode scmgn = Gaudi::svcLocator()->service("MagneticFieldSvc",m_pmgnIMF); 
    if(scmgn!=StatusCode::SUCCESS) { 
      std::cout<< __FILE__<<" "<<__LINE__<<" Unable to open Magnetic field service"<<std::endl;
    }

    //_fitter.setMagneticFieldPointer(m_pmgnIMF);//yzhang add 2012-05-04 
    //cout<<"TTrack: "<<m_pmgnIMF->getReferField()<<endl;
    //...Set a defualt fitter...
    fitter(& TTrack::_fitter);

    //...Calculate helix parameters...
    Vector a(5);
    a[1] = fmod(atan2(_charge * (c.center().y() - ORIGIN.y()),
                      _charge * (c.center().x() - ORIGIN.x()))
                + 4. * M_PI,
                2. * M_PI);
   // a[2] = Helix::ConstantAlpha / c.radius();
   // a[2] = 333.564095 / c.radius();
    const double Bz = -1000*m_pmgnIMF->getReferField();
    a[2] = 333.564095/ (c.radius() * Bz);
    a[0] = (c.center().x() - ORIGIN.x()) / cos(a[1]) - c.radius();
    a[3] = 0.;
    a[4] = 0.;
    _helix->a(a);

    //...Update links...
    unsigned n = _links.length();
    for (unsigned i = 0; i < n; i++)
        _links[i]->track(this);

    _fitted = false;
    _fittedWithCathode = false;
/*
    //jialk
    StatusCode scmgn = Gaudi::svcLocator()->service("MagneticFieldSvc",m_pmgnIMF); 
    if(scmgn!=StatusCode::SUCCESS) { 
      std::cout<< __FILE__<<" "<<__LINE__<<" Unable to open Magnetic field service"<<std::endl;
    }
*/
}

TTrack::TTrack(const TTrack & a)
: TTrackBase((TTrackBase &) a),
  _charge(a._charge),
  _segments(a._segments),
  _helix(new Helix(* a._helix)),
  _ndf(a._ndf),
  _chi2(a._chi2),
  _name("copy of " + a._name),
  _type(a._type),
//  _catHits(a._catHits),
  _state(a._state),
  _mother(a._mother),
  _daughter(a._daughter) { 
  //jialk
  StatusCode scmgn = Gaudi::svcLocator()->service("MagneticFieldSvc",m_pmgnIMF); 
  if(scmgn!=StatusCode::SUCCESS) { 
    std::cout<< __FILE__<<" "<<__LINE__<<" Unable to open Magnetic field service"<<std::endl;
  }
}

/*TTrack::TTrack(const T3DLine & a)
: TTrackBase((TTrackBase &) a),
  _charge(0),
  _helix(new Helix(* a.helix())),
  _ndf(a.ndf()),
  _chi2(a.chi2()),
  _name(0),
  _type(a.type()),
  _state(1),
  _mother(0),
  _daughter(0) {
}*/

TTrack::TTrack(const TRunge & a)
: TTrackBase((TTrackBase &) a),
  _helix(new Helix( a.helix())),
  _ndf(a.ndf()),
  _chi2(a.chi2()),
  _name("no"),
  _type(0),
  _state(0),
  _mother(0),
  _daughter(0) {
    if(_helix->kappa() > 0.)_charge = 1.;
        else _charge = -1.;
}
TTrack::TTrack(const Helix & h)
: TTrackBase(),
  _helix(new Helix(h)), 
  _ndf(0),
  _chi2(0.),
  _name("none"),
  _type(0),
  _state(0),
  _mother(0),
  _daughter(0) {

    //...Set a defualt fitter...
    fitter(& TTrack::_fitter);

    if(_helix->kappa() > 0.)_charge = 1.;
    else _charge = -1.;

    _fitted = false;
    _fittedWithCathode = false;
    //jialk
    StatusCode scmgn = Gaudi::svcLocator()->service("MagneticFieldSvc",m_pmgnIMF); 
    if(scmgn!=StatusCode::SUCCESS) { 
      std::cout<< __FILE__<<" "<<__LINE__<<" Unable to open Magnetic field service"<<std::endl;
    }
}

TTrack::TTrack()
: TTrackBase(),
  _charge(1.),
  _helix(new Helix(ORIGIN, Vector(5, 0), SymMatrix(5, 0))),
  _ndf(0),
  _chi2(0.),
  _name("empty track"),
  _state(0),
  _mother(0),
  _daughter(0) {
  //jialk
  StatusCode scmgn = Gaudi::svcLocator()->service("MagneticFieldSvc",m_pmgnIMF); 
  if(scmgn!=StatusCode::SUCCESS) { 
    std::cout<< __FILE__<<" "<<__LINE__<<" Unable to open Magnetic field service"<<std::endl;
  }
}

TTrack::~TTrack() {
    delete _helix;
}

void
TTrack::dump(const std::string & msg, const std::string & pre0) const {
    bool def = false;
    if (msg == "") def = true;
    std::string pre = pre0;
    std::string tab;
    for (unsigned i = 0; i < pre.length(); i++)
        tab += " ";
    if (def || msg.find("track") != std::string::npos || msg.find("detail") != std::string::npos) {
        std::cout << pre
                  << p() << "(pt=" << pt() << ")" << std::endl
                  << tab
                  << "links=" << _links.length()
                  << "(cores=" << nCores()
                  << "),chrg=" << _charge
                  << ",ndf=" << _ndf
                  << ",chi2=" << _chi2
                  << std::endl;
        pre = tab;
    }
    if (msg.find("helix") != std::string::npos || msg.find("detail") != std::string::npos) {
        std::cout << pre
                  << "pivot=" << _helix->pivot()
                  << ",center=" << _helix->center() << std::endl
                  << pre
                  << "helix=(" << _helix->a()[0] << "," << _helix->a()[1]<< ","
                  << _helix->a()[2] << "," << _helix->a()[3] << ","
                  << _helix->a()[4] << ")" << std::endl;
        pre = tab;
    }
    if (! def) TTrackBase::dump(msg, pre);
}

void
TTrack::movePivot(void) {
     unsigned n = _links.length();
    if (! n) {
        std::cout << "TTrack::movePivot !!! can't move a pivot"
                  << " because of no link";
        std::cout << std::endl;
        return;
    }

    //...Check cores...
     const AList<TMLink> & cores = TTrackBase::cores();
    const AList<TMLink> * links = & cores;
    if (cores.length() == 0) links = & _links;

    //...Hit loop...
    unsigned innerMost = 0;
    unsigned innerMostLayer = (* links)[0]->wire()->layerId();
    n = links->length();
    for (unsigned i = 1; i < n; i++) {
        TMLink * l = (* links)[i];
        if (l->wire()->layerId() < innerMostLayer) {
            innerMost = i;
            innerMostLayer = l->wire()->layerId();
        }
    }

    //...Move pivot...
    HepPoint3D newPivot = (* links)[innerMost]->positionOnWire();
    if (quality() & TrackQuality2D)
        newPivot.setZ(0.);
    _helix->pivot(newPivot);
}

int 
TTrack::approach(TMLink & l) const {
    return approach(l, false);
}

void
TTrack::refine2D(AList<TMLink> & list, float maxSigma) {
    unsigned n = _links.length();
    AList<TMLink> bad;
    for (unsigned i = 0; i < n; ++i) {
        if (_links[i]->pull() > maxSigma) bad.append(_links[i]);
    }
    _links.remove(bad);
    if (bad.length()){
      _fitted = false;
      fit2D();
    }
    list.append(bad);
}

int
TTrack::HelCyl(double rhole,
               double rCyl, 
               double zb,
               double zf,
               double epsl,
               double & phi,
               HepPoint3D & xp ) const {


  int status(0);        // return value
  //---------------------------------------------------------------------
  //  value     | ext |   status 
  //---------------------------------------------------------------------
  //     1.     |  OK |
  //    -1.     |  NO |  charge = 0
  //     0.     |  NO |  | tanl | < 0.1  ( neglect | lamda | < 5.7 deg. )
  //            |     |  or | dPhi | > 2 pi ( neglect curly track )
  //            |     |  or cannot reach to r=rhole at z = zb or zf.
  //     2.     |  OK |   backward , ext point set on z = zb
  //     3.     |  OK |   forward  , ext point set on z = zf
  //---------------------------------------------------------------------
  // * when value = 0,2,3 , ext(z) <= zb  or ext(z) >= zf

  //--- debug
  //  std::cout << "   "  << std::endl;
  //  std::cout << "HelCyl called ..  rhole=" << rhole << " rCyl=" << rCyl ; 
  //  std::cout << " zb=" << zb << " zf=" << zf << " epsl=" << epsl << std::endl;
  //--- debug end

  // Check of Charge

  if ( int(_charge) == 0 ) {
    std::cout << "HelCyl gets a straight line !!" << std::endl;
    return -1 ;
  }

  // parameters

  HepPoint3D CenterCyl( 0., 0., 0. );
  HepPoint3D CenterTrk = _helix->center();
  double  rTrk = fabs( _helix->radius() );

  double rPivot = fabs( _helix->pivot().perp() );

  double phi0 = _helix->a()[1];
  double tanl = _helix->a()[4];
  //  double zdz = _helix->pivot().z() + _helix->a()[3];
  double dPhi;
  double zee;


  // Calculate intersections between cylinder and track
  // if return value = 2 track hitting barrel part

  HepPoint3D Cross1, Cross2;

  if (intersection( CenterTrk,_charge * rTrk ,CenterCyl,rCyl,
                   epsl, 
                   Cross1,Cross2)
      == 2 ) {

    double phiCyl = atan2( _charge * ( CenterTrk.y() - Cross1.y() ),
                           _charge * ( CenterTrk.x() - Cross1.x() ) );
    phiCyl = ( phiCyl > 0. ) ? phiCyl : phiCyl + 2. * M_PI;

    dPhi = phiCyl - phi0;

    // dPhi region ( at cylinder )
    //          -pi <= dPhi < pi


    double PhiYobun = 1./ fabs( _helix->radius() );
    double zero = 0.00001;

    if( _charge >=0. ){
      if( dPhi > PhiYobun ) dPhi -= 2. * M_PI;
      if( -2. * M_PI  - zero <= dPhi <= ( -2.* M_PI + PhiYobun ) ) 
        dPhi += 2. * M_PI;
    }

    if( _charge < 0. ){
      if( dPhi < -PhiYobun ) dPhi += 2. * M_PI;
      if( 2. * M_PI + zero >= dPhi >= ( 2. * M_PI - PhiYobun ) )
        dPhi -= 2. * M_PI;
    }

    if( dPhi < - M_PI ) dPhi += 2. * M_PI;
    if( dPhi >= M_PI ) dPhi -= 2. * M_PI;

      //--debug 
      //  std::cout << "dPhi = " << dPhi << std::endl;
      //--debug end 

    xp.setX( Cross1.x() );
    xp.setY( Cross1.y() );
    xp.setZ( _helix->x(dPhi).z() );
    //    xp.setZ( zdz - _charge * rTrk * tanl * dPhi );

    if ( xp.z() > zb && xp.z() < zf ) {
      phi = dPhi;
 //--- debug ---
 //      std::cout << "return1 ( ext success )" <<  std::endl;
 //      std::cout << " xp:= " << xp.x() << ", " << xp.y() << ", " << xp.z() << std::endl;
  //--- debug  ----
      return 1 ;
    }
  }


  // tracks hitting endcaps

  if ( fabs(tanl) < 0.1 ) {
    //--- debug ---
    //    std::cout << "return0 ( ext failed , |tanl| < 0.1 )" <<  std::endl;
    //    std::cout << " xp:= " << xp.x() << ", " << xp.y() << ", " << xp.z() << std::endl;
    //--- debug ---
    return 0;
  }

  if ( tanl > 0. ) {
    zee = zf ;
    status = 3 ;
  } 
  else {
    zee = zb ;
    status = 2 ;
  }

  dPhi = _charge * ( _helix->x(0.).z() - zee )/rTrk/tanl;
  //  dPhi = _charge * ( zdz - zee )/rTrk/tanl;

  // Requre dPhi < 2*pi 

  if ( fabs(dPhi) > 2. * M_PI ) {
    //--- debug ---
    //    std::cout << " return0 ( ext failed , dPhi > 2pi )" << std::endl;
    //    std::cout << " xp:= " << xp.x() << ", " << xp.y() << ", " << xp.z() << std::endl;
    //--- debug  ---
    return 0 ;
  }

  xp.setX( _helix->x(dPhi).x() );
  xp.setY( _helix->x(dPhi).y() );
  xp.setZ( zee );

  double test = xp.perp2()  - rhole*rhole ;
  if ( test < 0. )  {
    //--- debug ---
    //    std::cout << "return0 ( cannot reach to rhole at z=edge )" << std::endl;
    //    std::cout << " xp:= " << xp.x() << ", " << xp.y() << ", " << xp.z() << std::endl;
    //--- debug ---
    return 0 ;
  }

  phi = dPhi ;
  //--- debug ---
  //  std::cout << "return" << status << std::endl;
  //  std::cout << " xp:= " << xp.x() << ", " << xp.y() << ", " << xp.z() << std::endl;
  //--- debug ---

  return status ;

}

/*
void
TTrack::findCatHit(unsigned trackid) {

  unsigned i = 0;
  //  while ( TMDC01CatHit * chit = _cathits.last() ) {
  //    _cathits.remove(chit);
  //    delete chit;
  //  }

  //--- debug ---
  //std::cout << std::endl << "FindCatHit  called !! " << std::endl << std::endl;
  //--- debug end ----

  // set cylinder geometry
  double rcyl[3];
  rcyl[0] = 8.80 ;
  rcyl[1] = 9.80 ;
  rcyl[2] = 10.85 ;
  double rhole = 8.00 ;
  double zb = -26.17 ;
  double zf =  45.87 ;
  double epsl = 0.01 ;

  // 
  HepPoint3D xp ;
  double phi ;
  TMDCCatHit * chit;

  // loop over layers and find cathits 
  
  for ( unsigned layer = 0 ; layer < 3 ; layer++ ) {
    if ( HelCyl (rhole, rcyl[layer], zb,zf,epsl, phi, xp ) == 1 ) {
      chit = new TMDCCatHit( this, trackid, layer, xp.x(), xp.y(), xp.z()) ;
      _catHits.append(chit); 
    }
  }

  //--- debug ---
  //if(_cathits.length()) {
  //  std::cout << std::endl;
  //  for ( int j = 0 ; j < _cathits.length() ; j++ ) {
  //    _cathits[j]->dump(" ", " ") ;
  //  }
  //}
  // chit->dump(" ", " ") ;
  //--- debug end ----

}
//--- Nagoya mods. 19981225 end ---------------------------


//  int
//  TTrack::fitx(void) {

//  #ifdef TRKRECO_DEBUG_DETAIL
//      std::cout << "TTrack::fit !!! This is obsolete !!!" << std::endl;
//  #endif
//      if (_fitted) return 0;

//      //...Check # of hits...
//      if (_links.length() < 5) return -1;

//      //...Setup...
//      unsigned nTrial = 0;
//      Vector a(5), da(5);
//      a = _helix->a();
//      Vector dxda(5);
//      Vector dyda(5);
//      Vector dzda(5);
//      Vector dDda(5);
//      Vector dchi2da(5);
//      SymMatrix d2chi2d2a(5, 0);
//      double chi2;
//      double chi2Old = 10e99;
//      const double convergence = 1.0e-5;
//      bool allAxial = true;
//      Matrix e(3, 3);
//      Vector f(3);
//      int err = 0;
//      double factor = 1.0;//jtanaka0715

//      Vector maxDouble(5);
//      for (unsigned i = 0; i < 5; i++) maxDouble[i] = (FLT_MAX);

//      //...Fitting loop...
//      while (nTrial < 100) {

//      //...Set up...
//      chi2 = 0.;
//      for (unsigned j = 0; j < 5; j++) dchi2da[j] = 0.;
//      d2chi2d2a = SymMatrix(5, 0);

//      //...Loop with hits...
//      unsigned i = 0;
//      while (TMLink * l = _links[i++]) {
//          const TMDCWireHit & h = * l->hit();

//          //...Check state...
//          if (h.state() & WireHitInvalidForFit) continue;

//          //...Check wire...
//          if (! nTrial)
//              if (h.wire()->stereo()) allAxial = false;

//          //...Cal. closest points...
//          approach(* l);
//          double dPhi = l->dPhi();
//          const HepPoint3D & onTrack = l->positionOnTrack();
//          const HepPoint3D & onWire = l->positionOnWire();

//  #ifdef TRKRECO_DEBUG_DETAIL
//  //      std::cout << "    in fit " << onTrack << ", " << onWire;
//  //      h.dump();
//  #endif          

//          //...Obtain drift distance and its error...
//          unsigned leftRight = WireHitRight;
//          if (onWire.cross(onTrack).z() < 0.) leftRight = WireHitLeft;
//          double distance = h.distance(leftRight);
//          double eDistance = h.dDistance(leftRight);
//          double eDistance2 = eDistance * eDistance;

//          //...Residual...
//          HepVector3D v = onTrack - onWire;
//          double vmag = v.mag();
//          double dDistance = vmag - distance;

//          //...dxda...
//          this->dxda(* l, dPhi, dxda, dyda, dzda);

//          //...Chi2 related...
//          // dDda = (v.x() * dxda + v.y() * dyda + v.z() * dzda) / vmag;
//          Vector3 vw = h.wire()->direction();
//              dDda = (vmag > 0.)
//              ? ((v.x() * (1. - vw.x() * vw.x()) -
//                  v.y() * vw.x() * vw.y() - v.z() * vw.x() * vw.z())
//                 * dxda + 
//                 (v.y() * (1. - vw.y() * vw.y()) -
//                  v.z() * vw.y() * vw.z() - v.x() * vw.y() * vw.x())
//                 * dyda + 
//                 (v.z() * (1. - vw.z() * vw.z()) -
//                  v.x() * vw.z() * vw.x() - v.y() * vw.z() * vw.y())
//                 * dzda) / vmag
//              : Vector(5,0);
//          if(vmag<=0.0) {
//            std::cout << "    in fit " << onTrack << ", " << onWire;
//            h.dump();
//          }
//          dchi2da += (dDistance / eDistance2) * dDda;
//          d2chi2d2a += vT_times_v(dDda) / eDistance2;
//          double pChi2 = dDistance * dDistance / eDistance2;
//          chi2 += pChi2;

//          //...Store results...
//          l->update(onTrack, onWire, leftRight, pChi2);
//      }

//      //...Check condition...
//      double change = chi2Old - chi2;
//      if (fabs(change) < convergence) break;
//      //if (change < 0.) break;
//      //jtanaka -- from traffs -- Ozaki-san added this part to traffs.        
//      if (change < 0.){
//  #ifdef TRKRECO_DEBUG_DETAIL
//            std::cout << "chi2Old, chi2=" << chi2Old <<" "<< chi2 << std::endl;
//  #endif
//        //change to the old value.
//            a += factor*da;
//            _helix->a(a);

//            chi2 = 0.;
//            for (unsigned j = 0; j < 5; j++) dchi2da[j] = 0.;
//            d2chi2d2a = SymMatrix(5, 0);
          
//            //...Loop with hits...
//            unsigned i = 0;
//            while (TMLink * l = _links[i++]) {
//            const TMDCWireHit & h = * l->hit();
            
//              //...Check wire...
//              if (! nTrial)
//                if (h.wire()->stereo()) allAxial = false;
            
//              //...Cal. closest points...
//              approach(* l);
//              double dPhi = l->dPhi();
//              const HepPoint3D & onTrack = l->positionOnTrack();
//              const HepPoint3D & onWire = l->positionOnWire();
            
//  #ifdef TRKRECO_DEBUG_DETAIL
//              // std::cout << "    in fit in case of change < 0. " << onTrack << ", " << onWire;
//              // h.dump();
//  #endif      
            
//              //...Obtain drift distance and its error...
//              unsigned leftRight = WireHitRight;
//              if (onWire.cross(onTrack).z() < 0.) leftRight = WireHitLeft;
//              double distance = h.distance(leftRight);
//              double eDistance = h.dDistance(leftRight);
//              double eDistance2 = eDistance * eDistance;
            
//              //...Residual...
//              HepVector3D v = onTrack - onWire;
//          double vmag = v.mag();
//          double dDistance = vmag - distance;
            
//              //...dxda...
//              this->dxda(* l, dPhi, dxda, dyda, dzda);
            
//              //...Chi2 related...
//              //dDda = (v.x() * dxda + v.y() * dyda + v.z() * dzda) / vmag;
//              Vector3 vw = h.wire()->direction();
//              dDda = (vmag > 0.)
//              ? ((v.x() * (1. - vw.x() * vw.x()) -
//                  v.y() * vw.x() * vw.y() - v.z() * vw.x() * vw.z())
//                 * dxda + 
//                 (v.y() * (1. - vw.y() * vw.y()) -
//                  v.z() * vw.y() * vw.z() - v.x() * vw.y() * vw.x())
//                 * dyda + 
//                 (v.z() * (1. - vw.z() * vw.z()) -
//                  v.x() * vw.z() * vw.x() - v.y() * vw.z() * vw.y())
//                 * dzda) / vmag
//              : Vector(5,0);
//          if(vmag<=0.0) {
//            std::cout << "    in fit " << onTrack << ", " << onWire;
//            h.dump();
//          }
//              dchi2da += (dDistance / eDistance2) * dDda;
//              d2chi2d2a += vT_times_v(dDda) / eDistance2;
//              double pChi2 = dDistance * dDistance / eDistance2;
//              chi2 += pChi2;
            
//              //...Store results...
//              l->update(onTrack, onWire, leftRight, pChi2);
//            }
//        //break;
//        factor *= 0.75;
//  #ifdef TRKRECO_DEBUG_DETAIL 
//        std::cout << "factor = " << factor << std::endl;
//        std::cout << "chi2 = " << chi2 << std::endl;
//  #endif
//        if(factor < 0.01)break;
//          }

//      chi2Old = chi2;

//      //...Cal. helix parameters for next loop...
//      if (allAxial) {
//          f = dchi2da.sub(1, 3);
//          e = d2chi2d2a.sub(1, 3);
//          f = solve(e, f);
//          da[0] = f[0];
//          da[1] = f[1];
//          da[2] = f[2];
//          da[3] = 0.;
//          da[4] = 0.;
//      }
//      else {
//          da = solve(d2chi2d2a, dchi2da);
//      }

//  #ifdef TRKRECO_DEBUG_DETAIL
//      //std::cout << "    fit " << nTrial << " : " << da << std::endl;
//      //std::cout << "        d2chi " << d2chi2d2a << std::endl;
//      //std::cout << "        dchi2 " << dchi2da << std::endl;
//  #endif          

//      a -= factor*da;
//      _helix->a(a);
//      ++nTrial;
//      }

//      //...Cal. error matrix...
//      SymMatrix Ea(5, 0);
//      unsigned dim;
//      if (allAxial) {
//      dim = 3;
//      SymMatrix Eb(3, 0), Ec(3, 0);
//      Eb = d2chi2d2a.sub(1, 3);
//      Ec = Eb.inverse(err);
//      Ea[0][0] = Ec[0][0];
//      Ea[0][1] = Ec[0][1];
//      Ea[0][2] = Ec[0][2];
//      Ea[1][1] = Ec[1][1];
//      Ea[1][2] = Ec[1][2];
//      Ea[2][2] = Ec[2][2];
//      }
//      else {
//      dim = 5;
//      Ea = d2chi2d2a.inverse(err);
//      }

//      //...Store information...
//      if (! err) {
//      _helix->a(a);
//      _helix->Ea(Ea);
//      }
    
//      _ndf = _links.length() - dim;
//      _chi2 = chi2;
    
//      _fitted = true;
//      return err;
//  }

*/

int
TTrack::dxda(const TMLink & link,
             double dPhi,
             Vector & dxda,
             Vector & dyda,
             Vector & dzda) const {

    //...Setup...
    const TMDCWire & w = * link.wire();
    Vector a = _helix->a();
    double dRho  = a[0];
    double phi0  = a[1];
    double kappa = a[2];
   // double rho   = Helix::ConstantAlpha / kappa;
   // double rho   = 333.564095 / kappa;
    const double Bz = -1000*m_pmgnIMF->getReferField();
    double rho   = 333.564095/(kappa * Bz); 
    double tanLambda = a[4];

    double sinPhi0 = sin(phi0);
    double cosPhi0 = cos(phi0);
    double sinPhi0dPhi = sin(phi0 + dPhi);
    double cosPhi0dPhi = cos(phi0 + dPhi);
    Vector dphida(5);

//yzhang 2012-08-30
    //...Axial case...
//    if (w.axial()) {
//      HepPoint3D d = _helix->center() - w.xyPosition();
//      double dmag2 = d.mag2();
//
//      dphida[0] = (sinPhi0 * d.x() - cosPhi0 * d.y()) / dmag2;
//      dphida[1] = (dRho + rho)    * (cosPhi0 * d.x() + sinPhi0 * d.y())
//          / dmag2 - 1.;
//      dphida[2] = (- rho / kappa) * (sinPhi0 * d.x() - cosPhi0 * d.y())
//          / dmag2;
//      dphida[3] = 0.;
//      dphida[4] = 0.;
//    }
//
//    //...Stereo case...
//    else {
        HepPoint3D onTrack = _helix->x(dPhi);
        HepVector3D v = w.direction();
        Vector c(3);
        c = HepPoint3D(w.backwardPosition() - (v * w.backwardPosition()) * v);

        Vector x(3);
        x = onTrack;

        Vector dxdphi(3);
        dxdphi[0] =   rho * sinPhi0dPhi;
        dxdphi[1] = - rho * cosPhi0dPhi;
        dxdphi[2] = - rho * tanLambda;

        Vector d2xdphi2(3);
        d2xdphi2[0] = rho * cosPhi0dPhi;
        d2xdphi2[1] = rho * sinPhi0dPhi;
        d2xdphi2[2] = 0.;

        double dxdphi_dot_v = dxdphi[0]*v.x() + dxdphi[1]*v.y() + dxdphi[2]*v.z();
        double x_dot_v      = x[0]*v.x() + x[1]*v.y() + x[2]*v.z();

        double dfdphi = - (dxdphi[0] - dxdphi_dot_v*v.x()) * dxdphi[0] 
                        - (dxdphi[1] - dxdphi_dot_v*v.y()) * dxdphi[1]
                        - (dxdphi[2] - dxdphi_dot_v*v.z()) * dxdphi[2]
                        - (x[0] - c[0] - x_dot_v*v.x()) * d2xdphi2[0]
                        - (x[1] - c[1] - x_dot_v*v.y()) * d2xdphi2[1];
                    /*  - (x[2] - c[2] - x_dot_v*v.z()) * d2xdphi2[2];  = 0. */


        //dxda_phi, dyda_phi, dzda_phi : phi is fixed
        Vector dxda_phi(5);
        dxda_phi[0] =  cosPhi0;
        dxda_phi[1] = -(dRho + rho)*sinPhi0 + rho*sinPhi0dPhi; 
        dxda_phi[2] = -(rho/kappa)*( cosPhi0 - cosPhi0dPhi );
        dxda_phi[3] =  0.;
        dxda_phi[4] =  0.;

        Vector dyda_phi(5);
        dyda_phi[0] =  sinPhi0;
        dyda_phi[1] =  (dRho + rho)*cosPhi0 - rho*cosPhi0dPhi;
        dyda_phi[2] = -(rho/kappa)*( sinPhi0 - sinPhi0dPhi );
        dyda_phi[3] =  0.;
        dyda_phi[4] =  0.;
        
        Vector dzda_phi(5);
        dzda_phi[0] =  0.;
        dzda_phi[1] =  0.;
        dzda_phi[2] =  (rho/kappa)*tanLambda*dPhi;
        dzda_phi[3] =  1.;
        dzda_phi[4] = -rho*dPhi;

        Vector d2xdphida(5);
        d2xdphida[0] =  0.;
        d2xdphida[1] =  rho*cosPhi0dPhi; 
        d2xdphida[2] = -(rho/kappa)*sinPhi0dPhi; 
        d2xdphida[3] =  0.;
        d2xdphida[4] =  0.;

        Vector d2ydphida(5);
        d2ydphida[0] = 0.;
        d2ydphida[1] = rho*sinPhi0dPhi;
        d2ydphida[2] = (rho/kappa)*cosPhi0dPhi; 
        d2ydphida[3] = 0.;
        d2ydphida[4] = 0.;

        Vector d2zdphida(5);
        d2zdphida[0] =  0.;
        d2zdphida[1] =  0.;
        d2zdphida[2] =  (rho/kappa)*tanLambda;
        d2zdphida[3] =  0.;
        d2zdphida[4] = -rho;
 
        Vector dfda(5);
        for( int i = 0; i < 5; i++ ){
          double d_dot_v = v.x()*dxda_phi[i] 
                         + v.y()*dyda_phi[i] 
                         + v.z()*dzda_phi[i];
          dfda[i] = - (dxda_phi[i] - d_dot_v*v.x()) * dxdphi[0]
                    - (dyda_phi[i] - d_dot_v*v.y()) * dxdphi[1]
                    - (dzda_phi[i] - d_dot_v*v.z()) * dxdphi[2]
                    - (x[0] - c[0] - x_dot_v*v.x()) * d2xdphida[i]
                    - (x[1] - c[1] - x_dot_v*v.y()) * d2ydphida[i]
                    - (x[2] - c[2] - x_dot_v*v.z()) * d2zdphida[i];
          dphida[i] = - dfda[i] /dfdphi;
        }
    //}

    dxda[0] = cosPhi0 + rho * sinPhi0dPhi * dphida[0];
    dxda[1] = - (dRho + rho) * sinPhi0 + rho * sinPhi0dPhi * (1. + dphida[1]);
    dxda[2] = - rho / kappa * (cosPhi0 - cosPhi0dPhi)
              + rho * sinPhi0dPhi * dphida[2];
    dxda[3] = rho * sinPhi0dPhi * dphida[3];
    dxda[4] = rho * sinPhi0dPhi * dphida[4];

    dyda[0] = sinPhi0 - rho * cosPhi0dPhi * dphida[0];
    dyda[1] = (dRho + rho) * cosPhi0 - rho * cosPhi0dPhi * (1. + dphida[1]);
    dyda[2] = - rho / kappa * (sinPhi0 - sinPhi0dPhi)
              - rho * cosPhi0dPhi * dphida[2];
    dyda[3] = - rho * cosPhi0dPhi * dphida[3];
    dyda[4] = - rho * cosPhi0dPhi * dphida[4];

    dzda[0] = - rho * tanLambda * dphida[0];
    dzda[1] = - rho * tanLambda * dphida[1];
    dzda[2] = rho / kappa * tanLambda * dPhi - rho * tanLambda * dphida[2];
    dzda[3] = 1. - rho * tanLambda * dphida[3];
    dzda[4] = - rho * dPhi - rho * tanLambda * dphida[4];
    
    return 0;
}

#define NEW_FIT2D 1
#if !(NEW_FIT2D)
int
TTrack::fit2D(void) {
#ifdef TRKRECO_DEBUG_DETAIL
  std::cout << "    TTrack::fit2D(r-phi) ..." << std::endl;
#endif
  if (_fitted) return 0;

  //...Check # of hits...
  if (_links.length() < 4) return -1;
  //std::cout << "# = " << _links.length() << std::endl;
  //...Setup...
  unsigned nTrial = 0;
  Vector a = _helix->a();
  Vector da(5), dxda(5), dyda(5), dzda(5);
  Vector dDda(5), dchi2da(5);
  SymMatrix d2chi2d2a(5, 0);
  double chi2;
  double chi2Old = 1.0e+99;
  const double convergence = 1.0e-5;
  Matrix e(3, 3);
  Vector f(3);
  int err = 0;
  double factor = 1.0;
  
  //...Fitting loop...
  while (nTrial < 100) {
#ifdef TRKRECO_DEBUG_DETAIL
    if(a[3] != 0. || a[4] != 0.)cerr << "Error in 2D functions of TTrack : a = " << a << std::endl;
#endif
    //...Set up...
    chi2 = 0.;
    for (unsigned j = 0; j < 5; ++j) dchi2da[j] = 0.;
    d2chi2d2a = SymMatrix(5, 0);

    //...Loop with hits...
    unsigned i = 0;
    while (TMLink * l = _links[i++]) {
      const TMDCWireHit & h = * l->hit();
      
      //...Cal. closest points...
      approach2D(* l);
      double dPhi = l->dPhi();
      const HepPoint3D & onTrack = l->positionOnTrack();
      const HepPoint3D & onWire = l->positionOnWire();
#ifdef TRKRECO_DEBUG_DETAIL
      if(onTrack.z() != 0.)cerr << "Error in 2D functions of TTrack : onTrack = " << onTrack << std::endl;
      if(onWire.z() != 0.)cerr << "Error in 2D functions of TTrack : onWire = " << onWire << std::endl;
#endif
      HepPoint3D onTrack2(onTrack.x(),onTrack.y(),0.);
      HepPoint3D onWire2(onWire.x(),onWire.y(),0.);

      //...Obtain drift distance and its error...
      unsigned leftRight = WireHitRight;
      if (onWire2.cross(onTrack2).z() < 0.) leftRight = WireHitLeft;
      double distance = h.distance(leftRight);
      double eDistance = h.dDistance(leftRight);
      double eDistance2 = eDistance * eDistance;

      //...Residual...
      HepVector3D v = onTrack2 - onWire2;
      double vmag = v.mag();
      double dDistance = vmag - distance;

      //...dxda...
      this->dxda2D(* l, dPhi, dxda, dyda, dzda);

      //...Chi2 related...
      //Vector3 vw(0.,0.,1.);
      dDda = (vmag > 0.)
        ? (v.x() * dxda + 
           v.y() * dyda ) / vmag
        : Vector(5,0);
      //dDda = (vmag > 0.)
      //? ((v.x() * (1. - vw.x() * vw.x()) -
      //    v.y() * vw.x() * vw.y() - v.z() * vw.x() * vw.z())
      //   * dxda + 
      //   (v.y() * (1. - vw.y() * vw.y()) -
      //    v.z() * vw.y() * vw.z() - v.x() * vw.y() * vw.x())
      //   * dyda + 
      //   (v.z() * (1. - vw.z() * vw.z()) -
      //    v.x() * vw.z() * vw.x() - v.y() * vw.z() * vw.y())
      //   * dzda) / vmag
      //: Vector(5,0);
      if(vmag<=0.0) {
        std::cout << "    in fit2D " << onTrack << ", " << onWire;
        h.dump();
      }
      dchi2da += (dDistance / eDistance2) * dDda;
      d2chi2d2a += vT_times_v(dDda) / eDistance2;
      double pChi2 = dDistance * dDistance / eDistance2;
      chi2 += pChi2;
      
      //...Store results...
      l->update(onTrack2, onWire2, leftRight, pChi2);
    }

    //...Check condition...
    double change = chi2Old - chi2;
    if (fabs(change) < convergence) break;
    if (change < 0.){
#ifdef TRKRECO_DEBUG_DETAIL
      std::cout << "chi2Old, chi2=" << chi2Old <<" "<< chi2 << std::endl;
#endif
      //change to the old value.
      a += factor*da;
      _helix->a(a);

      chi2 = 0.;
      for (unsigned j = 0; j < 5; ++j) dchi2da[j] = 0.;
      d2chi2d2a = SymMatrix(5, 0);
          
      //...Loop with hits...
      unsigned i = 0;
      while (TMLink * l = _links[i++]) {
        const TMDCWireHit & h = * l->hit();
        
        //...Cal. closest points...
        approach2D(* l);
        double dPhi = l->dPhi();
        const HepPoint3D & onTrack = l->positionOnTrack();
        const HepPoint3D & onWire = l->positionOnWire();
        HepPoint3D onTrack2(onTrack.x(),onTrack.y(),0.);
        HepPoint3D onWire2(onWire.x(),onWire.y(),0.);
        
        //...Obtain drift distance and its error...
        unsigned leftRight = WireHitRight;
        if (onWire2.cross(onTrack2).z() < 0.) leftRight = WireHitLeft;
        double distance = h.distance(leftRight);
        double eDistance = h.dDistance(leftRight);
        double eDistance2 = eDistance * eDistance;
            
        //...Residual...
        HepVector3D v = onTrack2 - onWire2;
        double vmag = v.mag();
        double dDistance = vmag - distance;
            
        //...dxda...
        this->dxda2D(* l, dPhi, dxda, dyda, dzda);

        //...Chi2 related...
        dDda = (vmag > 0.)
          ? (v.x() * dxda + 
             v.y() * dyda ) / vmag
          : Vector(5,0);
        if(vmag<=0.0) {
          std::cout << "    in fit2D " << onTrack << ", " << onWire;
          h.dump();
        }
        dchi2da += (dDistance / eDistance2) * dDda;
        d2chi2d2a += vT_times_v(dDda) / eDistance2;
        double pChi2 = dDistance * dDistance / eDistance2;
        chi2 += pChi2;
        
        //...Store results...
        l->update(onTrack2, onWire2, leftRight, pChi2);
      }
      //break;
      factor *= 0.75;
#ifdef TRKRECO_DEBUG_DETAIL 
      std::cout << "factor = " << factor << std::endl;
      std::cout << "chi2 = " << chi2 << std::endl;
#endif
      if(factor < 0.01)break;
    }

    chi2Old = chi2;

    //...Cal. helix parameters for next loop...
    f = dchi2da.sub(1, 3);
    e = d2chi2d2a.sub(1, 3);
    f = solve(e, f);
    da[0] = f[0];
    da[1] = f[1];
    da[2] = f[2];
    da[3] = 0.;
    da[4] = 0.;
    
    a -= factor*da;
    _helix->a(a);      
    //std::cout << nTrial << ": chi2 = " << chi2 
    // << ", helix = (" << a[0] << ", " << a[1] 
    // << ", " << a[2] << ", " << a[3] 
    // << ", " << a[4] << ")" << std::endl;
    ++nTrial;
  }

  //...Cal. error matrix...
  SymMatrix Ea(5, 0);
  unsigned dim = 3;
  SymMatrix Eb = d2chi2d2a.sub(1, 3);
  SymMatrix Ec = Eb.inverse(err);
  Ea[0][0] = Ec[0][0];
  Ea[0][1] = Ec[0][1];
  Ea[0][2] = Ec[0][2];
  Ea[1][1] = Ec[1][1];
  Ea[1][2] = Ec[1][2];
  Ea[2][2] = Ec[2][2];

  //...Store information...
  if (! err) {
    _helix->a(a);
    _helix->Ea(Ea);
  }
    
  _ndf = _links.length() - dim;
  _chi2 = chi2;
    
  _fitted = true;
  return err;
}
#endif

/*
int TTrack::fitWithCathode(float window, int SysCorr ) {

#ifdef TRKRECO_DEBUG_DETAIL
    std::cout << "    TTrack::fitCathode ..." << std::endl;
#endif

    if (_fittedWithCathode) return 0;

    //...Check # of hits...
    if (nCores() < 5) return -1;

    //...for cathode ndf...
    int NusedCathode(0);

    //...Setup...
    unsigned nTrial = 0;
    Vector a(5), da(5);
    a = _helix->a();
    Vector dxda(5);
    Vector dyda(5);
    Vector dzda(5);
    Vector dDda(5);
    Vector dDzda(5);  // for cathode z
    Vector dchi2da(5);
    SymMatrix d2chi2d2a(5, 0);
    double chi2;
    double chi2Old = 10e99;
    const double convergence = 1.0e-5;
    bool allAxial = true;
    int LayerStat(0); // layer status  axial:0 stereo:1 cathode:2
    Matrix e(3, 3);
    Vector f(3);
    int err = 0;
    double factor = 1.0;

    const AList<TMDCCatHit> & chits = _catHits;

    Vector maxDouble(5);
    for (unsigned i = 0; i < 5; i++) maxDouble[i] = (FLT_MAX);

    //...Fitting loop...
    while (nTrial < 100) {

      //...Set up...
      chi2 = 0.;
      NusedCathode = 0;
      for (unsigned j = 0; j < 5; j++) dchi2da[j] = 0.;
      d2chi2d2a = SymMatrix(5, 0);

      //...Loop with hits...
      unsigned i = 0;
      while (TMLink * l = cores()[i++]) {

          const TMDCWireHit & h = * l->hit();

        // Check layer status ( cathode added ) 
        LayerStat = 0;
        if ( h.wire()->stereo() ) LayerStat = 1;
        unsigned nlayer = h.wire()->layerId();
        if (nlayer == 0 || nlayer == 1 || nlayer == 2 ) LayerStat = 2;

        //...Check wire...
        if (! nTrial)
          if (h.wire()->stereo() || LayerStat == 2 ) allAxial = false;

        //...Cal. closest points...
        approach(* l);
        double dPhi = l->dPhi();
        const HepPoint3D & onTrack = l->positionOnTrack();
        const HepPoint3D & onWire = l->positionOnWire();

#ifdef TRKRECO_DEBUG_DETAIL
        // std::cout << "    in fitCathode " << onTrack << ", " << onWire;
        // h.dump();
#endif      

        //...Obtain drift distance and its error...
        unsigned leftRight = WireHitRight;
        if (onWire.cross(onTrack).z() < 0.)     leftRight = WireHitLeft;
        double distance = h.drift(leftRight);
        double eDistance = h.dDrift(leftRight);
        double eDistance2 = eDistance * eDistance;

        //...Residual...
        HepVector3D v = onTrack - onWire;
        double vmag = v.mag();
        double dDistance = vmag - distance;

        //...dxda...
        this->dxda(* l, dPhi, dxda, dyda, dzda);

        // ... Chi2 related ...
        double pChi2(0.);

        if( LayerStat == 0 || LayerStat == 1 ){
            // dDda = (v.x() * dxda + v.y() * dyda + v.z() * dzda) / vmag;
            Vector3 vw = h.wire()->direction();
            dDda = (vmag > 0.)
                ? ((v.x() * (1. - vw.x() * vw.x()) -
                    v.y() * vw.x() * vw.y() - v.z() * vw.x() * vw.z())
                   * dxda + 
                   (v.y() * (1. - vw.y() * vw.y()) -
                    v.z() * vw.y() * vw.z() - v.x() * vw.y() * vw.x())
                   * dyda + 
                   (v.z() * (1. - vw.z() * vw.z()) -
                    v.x() * vw.z() * vw.x() - v.y() * vw.z() * vw.y())
                   * dzda) / vmag
                :  Vector(5,0);
            if(vmag<=0.0) {
              std::cout << "    in fit " << onTrack << ", " << onWire;
              h.dump();
            }
          dchi2da += (dDistance / eDistance2) * dDda;
          d2chi2d2a += vT_times_v(dDda) / eDistance2;
          double pChi2 = dDistance * dDistance / eDistance2;
          chi2 += pChi2;

        } else {


          dDda = ( v.x() * dxda + v.y() * dyda )/v.perp();
          dchi2da += (dDistance/eDistance2) * dDda;
          d2chi2d2a += vT_times_v(dDda)/eDistance2;

          pChi2 = 0;
          pChi2 += (dDistance/eDistance) * (dDistance/eDistance) ;

          if ( l->usecathode() >= 3 ) {

            TMDCClust * mclust = l->getmclust();

              double dDistanceZ(this->helix().x(dPhi).z());

              if( SysCorr ){
                if( !nTrial ) {
                  mclust->zcalc( atan( this->helix().tanl()) );
                  mclust->esterz( atan( this->helix().tanl()) );
                }

                dDistanceZ -= mclust->zclustWithSysCorr();
             } else {
                dDistanceZ -= mclust->zclust();
             }
              
             double eDistanceZ(mclust->erz());
             if( !eDistanceZ ) eDistanceZ = 99999.;

              double eDistance2Z = eDistanceZ * eDistanceZ;
              double pChi2z = 0;
              pChi2z = (dDistanceZ/eDistanceZ);
              pChi2z *= pChi2z;

#ifdef TRKRECO_DEBUG_DETAIL
              std::cout << "dDistanceZ = " << dDistanceZ << std::endl;
#endif

      //.... requirement for use of cluster

      if( nTrial == 0 && 
          fabs(dDistanceZ)< window && 
          mclust->chits().length() == 1 
          ) l->setusecathode(4);

      if( l->usecathode() == 4 ){
                NusedCathode++;
                dDzda = dzda ; 
                dchi2da += (dDistanceZ/eDistance2Z) * dDzda;
                d2chi2d2a += vT_times_v(dDzda)/eDistance2Z;
                pChi2 +=  pChi2z ;

            }
          }
        
        } // end Chi2 related

        chi2 += pChi2;


        //...Store results...
        l->update(onTrack, onWire, leftRight, pChi2);
      

      } // Tlink loop end

      //...Check condition...
      double change = chi2Old - chi2;
      //if(TRKRECO_DEBUG_DETAIL>0)  std::cout << "chi2 change = " <<change << std::endl;
   
      if (fabs(change) < convergence) break;
      if (change < 0.) {

#ifdef TRKRECO_DEBUG_DETAIL
          std::cout << "chi2Old, chi2=" << chi2Old <<" "<< chi2 << std::endl;
#endif

          NusedCathode = 0;
          //change to the old value.
          a += factor*da;
          _helix->a(a);

          chi2 = 0.;
          for (unsigned j = 0; j < 5; j++) dchi2da[j] = 0.;
          d2chi2d2a = SymMatrix(5, 0);

          
          //...Loop with hits...
          unsigned i = 0;
          while (TMLink * l = _links[i++]) {
              const TMDCWireHit & h = * l->hit();

     // Check layer status ( cathode added )          
        LayerStat = 0;
        if ( h.wire()->stereo() ) LayerStat = 1;
        unsigned nlayer = h.wire()->layerId();
        if (nlayer == 0 || nlayer == 1 || nlayer == 2 ) LayerStat = 2;

        //...Cal. closest points...
        approach(* l);
        double dPhi = l->dPhi();
        const HepPoint3D & onTrack = l->positionOnTrack();
        const HepPoint3D & onWire = l->positionOnWire();

#ifdef TRKRECO_DEBUG_DETAIL
        // std::cout << "    in fitCathode " << onTrack << ", " << onWire;
        // h.dump();
#endif      

        //...Obtain drift distance and its error...
        unsigned leftRight = WireHitRight;
        if (onWire.cross(onTrack).z() < 0.)     leftRight = WireHitLeft;
        double distance = h.drift(leftRight);
        double eDistance = h.dDrift(leftRight);
        double eDistance2 = eDistance * eDistance;

        //...Residual...
        HepVector3D v = onTrack - onWire;
        double vmag = v.mag();
        double dDistance = vmag - distance;

        //...dxda...
        this->dxda(* l, dPhi, dxda, dyda, dzda);

        // ... Chi2 related ...
        double pChi2(0.);

        if( LayerStat == 0 || LayerStat == 1 ){
            // dDda = (v.x() * dxda + v.y() * dyda + v.z() * dzda) / vmag;
            Vector3 vw = h.wire()->direction();
            dDda = (vmag > 0.)
                ? ((v.x() * (1. - vw.x() * vw.x()) -
                    v.y() * vw.x() * vw.y() - v.z() * vw.x() * vw.z())
                   * dxda + 
                   (v.y() * (1. - vw.y() * vw.y()) -
                    v.z() * vw.y() * vw.z() - v.x() * vw.y() * vw.x())
                   * dyda + 
                   (v.z() * (1. - vw.z() * vw.z()) -
                    v.x() * vw.z() * vw.x() - v.y() * vw.z() * vw.y())
                   * dzda) / vmag
                : Vector(5,0);
            if(vmag<=0.0) {
              std::cout << "    in fit " << onTrack << ", " << onWire;
              h.dump();
            }
          dchi2da += (dDistance / eDistance2) * dDda;
          d2chi2d2a += vT_times_v(dDda) / eDistance2;
          double pChi2 = dDistance * dDistance / eDistance2;
          chi2 += pChi2;

        } else {

          dDda = ( v.x() * dxda + v.y() * dyda )/v.perp();
          dchi2da += (dDistance/eDistance2) * dDda;
          d2chi2d2a += vT_times_v(dDda)/eDistance2;

          pChi2 = 0;
          pChi2 += (dDistance/eDistance) * (dDistance/eDistance) ;

          if( l->usecathode() == 4 ){
            
            TMDCClust * mclust = l->getmclust();

            if( mclust ){
                NusedCathode++;

               double dDistanceZ(this->helix().x(dPhi).z());
               if( SysCorr ) dDistanceZ -= mclust->zclustWithSysCorr();
               else          dDistanceZ -= mclust->zclust();

              double eDistanceZ(99999.);
              if( mclust->erz() != 0. ) eDistanceZ = mclust->erz();

              double eDistance2Z = eDistanceZ * eDistanceZ;
              double pChi2z = 0;
              pChi2z = (dDistanceZ/eDistanceZ);
              pChi2z *= pChi2z;

                dDzda = dzda ; 
                dchi2da += (dDistanceZ/eDistance2Z) * dDzda;
                d2chi2d2a += vT_times_v(dDzda)/eDistance2Z;
                pChi2 +=  pChi2z ;
            }

           }
        
        } // end Chi2 related

        chi2 += pChi2;


        //...Store results...
        l->update(onTrack, onWire, leftRight, pChi2);
      
          }

           //break;

          factor *= 0.75;
#ifdef TRKRECO_DEBUG_DETAIL 
          std::cout << "factor = " << factor << std::endl;
          std::cout << "chi2 = " << chi2 << std::endl;
#endif
          if(factor < 0.01)break;

      }

      chi2Old = chi2;
      
      //...Cal. helix parameters for next loop...
      if (allAxial) {
        f = dchi2da.sub(1, 3);
        e = d2chi2d2a.sub(1, 3);
        f = solve(e, f);
        da[0] = f[0];
        da[1] = f[1];
        da[2] = f[2];
        da[3] = 0.;
        da[4] = 0.;
      }
      else {
        da = solve(d2chi2d2a, dchi2da);
      }
      
#ifdef TRKRECO_DEBUG_DETAIL
      //std::cout << "    fit " << nTrial << " : " << da << std::endl;
      //std::cout << "        d2chi " << d2chi2d2a << std::endl;
      //std::cout << "        dchi2 " << dchi2da << std::endl;
#endif      
      
      a -= da;
      _helix->a(a);
      ++nTrial;
    }

    //...Cal. error matrix...
    SymMatrix Ea(5, 0);
    unsigned dim;
    if (allAxial) {
      dim = 3;
      SymMatrix Eb(3, 0), Ec(3, 0);
      Eb = d2chi2d2a.sub(1, 3);
      Ec = Eb.inverse(err);
      Ea[0][0] = Ec[0][0];
      Ea[0][1] = Ec[0][1];
      Ea[0][2] = Ec[0][2];
      Ea[1][1] = Ec[1][1];
      Ea[1][2] = Ec[1][2];
      Ea[2][2] = Ec[2][2];
    }
    else {
      dim = 5;
      Ea = d2chi2d2a.inverse(err);
    }
    
    //...Store information...
    if (! err) {
      _helix->a(a);
      _helix->Ea(Ea);
    }
      
    _ndf = nCores() + NusedCathode - dim;
    _chi2 = chi2;

    _fittedWithCathode = true;
    
    return err;
}
*/

#if OLD_STEREO
int
TTrack::stereoHitForCurl(TMLink & link, AList<HepPoint3D> & arcZList) const 
{
  /*
    ATTENTION!!!!!
    Elements of arcZList are made by "new". We must delete them out of this function!!!!!

    This function is used in "stereoHitForCurl(TMLink &link1, TMLink &link2)" and
    "stereoHitForCurl(TMLink &link1, TMLink &link2, TMLink &link3)" only.
    (I(jtanaka) checked it. --> no memory leak!!!)

    */
  
    //std::cout << "\n\nWire ID = " << link.hit()->wire()->id() << std::endl;
    const TMDCWireHit &h = *link.hit();
    HepVector3D X     = 0.5*(h.wire()->forwardPosition() +
                                  h.wire()->backwardPosition());
    
    HepPoint3D center = _helix->center();
    HepPoint3D tmp(-999., -999., 0.);
    HepVector3D x     = HepVector3D(X.x(), X.y(), 0.);
    HepVector3D w     = x - center;
    HepVector3D V     = h.wire()->direction();
    HepVector3D v     = HepVector3D(V.x(), V.y(), 0.);
    double   vmag2 = v.mag2();
    double   vmag  = sqrt(vmag2);
    //...temporary
    link.position(tmp);
    arcZList.removeAll();

    //...stereo?
    if (vmag == 0.){
        return -1;
    }

    double r  = _helix->curv();
    double R[2];
    double drift = h.drift(WireHitLeft);
    R[0] = r + drift;
    R[1] = r - drift;
    double wv = w.dot(v);
    double d2[2];
    d2[0] = vmag2*R[0]*R[0] + (wv*wv - vmag2*w.mag2()); //...= v^2*(r^2 - w^2*sin()^2)...outer
    d2[1] = vmag2*R[1]*R[1] + (wv*wv - vmag2*w.mag2()); //...= v^2*(r^2 - w^2*sin()^2)...inner

    //...No crossing in R/Phi plane...
    if (d2[0] < 0. && d2[1] < 0.) {
        return -1;
    }

    bool ok_inner, ok_outer;
    ok_inner = true;
    ok_outer = true;
    double d[2];
    d[0] = -1.; 
    d[1] = -1.;
    //...outer
    if(d2[0] >= 0.){
        d[0] = sqrt(d2[0]);
    }else{
        ok_outer = false;
    }
    if(d2[1] >= 0.){
        d[1] = sqrt(d2[1]);
    }else{
        ok_inner = false;
    }
    
    //...Cal. length and z to crossing points...
    double l[2][2];
    double z[2][2];
    //...outer
    if(ok_outer){
        l[0][0] = (- wv + d[0]) / vmag2; //...= (-wvcos()+d)/v/v = (-wcos() + (r^2 - w^2*sin()^2)^0.5)/v
        l[1][0] = (- wv - d[0]) / vmag2; //...= (-wvcos()+d)/v/v = (-wcos() - (r^2 - w^2*sin()^2)^0.5)/v
        z[0][0] = X.z() + l[0][0]*V.z();
        z[1][0] = X.z() + l[1][0]*V.z();
    }
    //...inner
    if(ok_inner){
        l[0][1] = (- wv + d[1]) / vmag2; //...= (-wvcos()+d)/v/v = (-wcos() + (r^2 - w^2*sin()^2)^0.5)/v
        l[1][1] = (- wv - d[1]) / vmag2; //...= (-wvcos()+d)/v/v = (-wcos() - (r^2 - w^2*sin()^2)^0.5)/v
        z[0][1] = X.z() + l[0][1]*V.z();
        z[1][1] = X.z() + l[1][1]*V.z();
    }

    //...Cal. xy position of crossing points...
    HepVector3D p[2][2];
    if(ok_outer){
        p[0][0] = x + l[0][0] * v;
        p[1][0] = x + l[1][0] * v;
        HepVector3D tmp_pc = p[0][0] - center;
        HepVector3D pc0 = HepVector3D(tmp_pc.x(), tmp_pc.y(), 0.);
        p[0][0] -= drift/pc0.mag()*pc0;
        tmp_pc = p[1][0] - center;
        HepVector3D pc1 = HepVector3D(tmp_pc.x(), tmp_pc.y(), 0.);
        p[1][0] -= drift/pc1.mag()*pc1;
    }
    if(ok_inner){
        p[0][1] = x + l[0][1] * v;
        p[1][1] = x + l[1][1] * v;
        HepVector3D tmp_pc = p[0][1] - center;
        HepVector3D pc0 = HepVector3D(tmp_pc.x(), tmp_pc.y(), 0.);
        p[0][1] += drift/pc0.mag()*pc0;
        tmp_pc = p[1][1] - center;
        HepVector3D pc1 = HepVector3D(tmp_pc.x(), tmp_pc.y(), 0.);
        p[1][1] += drift/pc1.mag()*pc1;
    }

    //...boolean
    bool ok_xy[2][2];
    if(ok_outer){
        ok_xy[0][0] = true;
        ok_xy[1][0] = true;
    }else{
        ok_xy[0][0] = false;
        ok_xy[1][0] = false;
    }
    if(ok_inner){
        ok_xy[0][1] = true;
        ok_xy[1][1] = true;
    }else{
        ok_xy[0][1] = false;
        ok_xy[1][1] = false;
    }
    if(ok_outer){
        if (_charge * (center.x() * p[0][0].y() - center.y() * p[0][0].x())  < 0.)
          ok_xy[0][0] = false;
        if (_charge * (center.x() * p[1][0].y() - center.y() * p[1][0].x())  < 0.)
          ok_xy[1][0] = false;
    }
    if(ok_inner){
        if (_charge * (center.x() * p[0][1].y() - center.y() * p[0][1].x())  < 0.)
          ok_xy[0][1] = false;
        if (_charge * (center.x() * p[1][1].y() - center.y() * p[1][1].x())  < 0.)
          ok_xy[1][1] = false;
    }
    if(!ok_inner && ok_outer && (!ok_xy[0][0]) && (!ok_xy[1][0])){
        return -1;
    }
    if(ok_inner && !ok_outer && (!ok_xy[0][1]) && (!ok_xy[1][1])){
        return -1;
    }

#if 0
    std::cout << "Drift Dist. = " << h.distance(WireHitLeft) << std::endl;
    std::cout << "outer ok? = " << ok_outer << std::endl; 
    std::cout << "Z cand = " << z[0][0] << ", " << z[1][0] << std::endl;
    std::cout << "inner ok? = " << ok_inner << std::endl; 
    std::cout << "Z cand = " << z[0][1] << ", " << z[1][1] << std::endl; 
#endif

    //...Check z position...
    if(ok_xy[0][0]){
        if (z[0][0] < h.wire()->backwardPosition().z() || 
            z[0][0] > h.wire()->forwardPosition().z()) ok_xy[0][0] = false;
    }
    if(ok_xy[1][0]){
        if (z[1][0] < h.wire()->backwardPosition().z() || 
            z[1][0] > h.wire()->forwardPosition().z()) ok_xy[1][0] = false;
    }
    if(ok_xy[0][1]){
        if (z[0][1] < h.wire()->backwardPosition().z() || 
            z[0][1] > h.wire()->forwardPosition().z()) ok_xy[0][1] = false;
    }
    if(ok_xy[1][1]){
        if (z[1][1] < h.wire()->backwardPosition().z() || 
            z[1][1] > h.wire()->forwardPosition().z()) ok_xy[1][1] = false;
    }
    if ((!ok_xy[0][0]) && (!ok_xy[1][0]) &&
          (!ok_xy[0][1]) && (!ok_xy[1][1])){
        return -1;
    }

    double cosdPhi, dPhi;

    if(ok_xy[0][0]){
        //...cal. arc length...
        cosdPhi = - center.dot((p[0][0] - center).unit()) / center.mag();
        if(fabs(cosdPhi) < 1.0){
          dPhi = acos(cosdPhi);
        }else if(cosdPhi >= 1.0){
          dPhi = 0.;
        }else{
          dPhi = M_PI;
        }
    
        HepPoint3D *arcZ = new HepPoint3D;
        arcZ->setX(r*dPhi);
        arcZ->setY(z[0][0]);
        arcZList.append(arcZ);
    }
    if(ok_xy[1][0]){
        //...cal. arc length...
        cosdPhi = - center.dot((p[1][0] - center).unit()) / center.mag();
        if(fabs(cosdPhi) < 1.0){
          dPhi = acos(cosdPhi);
        }else if(cosdPhi >= 1.0){
          dPhi = 0.;
        }else{
          dPhi = M_PI;
        }
        
        HepPoint3D *arcZ = new HepPoint3D;
        arcZ->setX(r*dPhi);
        arcZ->setY(z[1][0]);
        arcZList.append(arcZ);
    }
    if(ok_xy[0][1]){
        //...cal. arc length...
        cosdPhi = - center.dot((p[0][1] - center).unit()) / center.mag();
        if(cosdPhi>1.0) cosdPhi = 1.0;
        if(cosdPhi<-1.0) cosdPhi = -1.0;

        if(fabs(cosdPhi) < 1.0){
          dPhi = acos(cosdPhi);
        }else if(cosdPhi >= 1.0){
          dPhi = 0.;
        }else{
          dPhi = M_PI;
        }
        
        HepPoint3D *arcZ = new HepPoint3D;
        arcZ->setX(r*dPhi);
        arcZ->setY(z[0][1]);
        arcZList.append(arcZ);
    }
    if(ok_xy[1][1]){
        //...cal. arc length...
        cosdPhi = - center.dot((p[1][1] - center).unit()) / center.mag();
        if(cosdPhi>1.0) cosdPhi = 1.0;
        if(cosdPhi<-1.0) cosdPhi = -1.0;
        if(fabs(cosdPhi) < 1.0){
          dPhi = acos(cosdPhi);
        }else if(cosdPhi >= 1.0){
          dPhi = 0.;
        }else{
          dPhi = M_PI;
        }
        
        HepPoint3D *arcZ = new HepPoint3D;
        arcZ->setX(r*dPhi);
        arcZ->setY(z[1][1]);
        arcZList.append(arcZ);
    }
    
    if(arcZList.length() == 1 ||
         arcZList.length() == 2){
        return 0;
    }else{
        return -1;
    } 
}


int
TTrack::stereoHitForCurl(TMLink &link1, TMLink &link2) const {
  int flag1, flag2;
  double minZ = 9999999.;

  AList<HepPoint3D> arcZList1;
  AList<HepPoint3D> arcZList2;
  if(stereoHitForCurl(link1,arcZList1) == 0){
    if(stereoHitForCurl(link2,arcZList2) == 0){
        //...finds
        int n1 = arcZList1.length();
        int n2 = arcZList2.length();
        for(int i=0;i<n1;i++){
          for(int j=0;j<n2;j++){
            if(fabs(arcZList1[i]->y()-arcZList2[j]->y()) < minZ){
                minZ = fabs(arcZList1[i]->y()-arcZList2[j]->y());
                flag1 = i;
                flag2 = j;
            }
          }
        }
    }
  }
  
  if(minZ == 9999999.){
    //...deletes
    deleteListForCurl(arcZList1, arcZList2);
    return -1;
  }

  //...sets the best values
  HepPoint3D tmp1 = *arcZList1[flag1];
  HepPoint3D tmp2 = *arcZList2[flag2];
  link1.position(tmp1);
  link2.position(tmp2);
  //...deletes
  deleteListForCurl(arcZList1, arcZList2);
  return 0;
}

#if defined(__GNUG__)
int 
TArcOrder( const HepPoint3D **a, const HepPoint3D **b )
{
  if( (*a)->x() < (*b)->x() ){
    return 1;
  }else if( (*a)->x() == (*b)->x() ){
    return 0;
  }else{
    return -1;
  }
}
#else
extern "C" int 
TArcOrder( const void *av, const void *bv )
{
  const HepPoint3D **a((const HepPoint3D **)av);
  const HepPoint3D **b((const HepPoint3D **)bv);
  if( (*a)->x() < (*b)->x() ){
    return 1;
  }else if( (*a)->x() == (*b)->x() ){
    return 0;
  }else{
    return -1;
  }
}
#endif

int
TTrack::stereoHitForCurl(TMLink &link1, TMLink &link2, TMLink &link3) const 
{
  //...definition of the return values
  //   -1 = error
  //    0 = normal = can use 3 links
  //   12 = can use 1 and 2 only
  //   23 = can use 2 and 3 only

  int flag1, flag2, flag3;
  double minZ1   = 9999999.;
  double minZ2   = 9999999.;
  double minZ01  = 9999999.;
  double minZ12  = 9999999.;

  AList<HepPoint3D> arcZList1;
  AList<HepPoint3D> arcZList2;
  AList<HepPoint3D> arcZList3;
  int ok1 = stereoHitForCurl(link1, arcZList1);
  int ok2 = stereoHitForCurl(link2, arcZList2);
  int ok3 = stereoHitForCurl(link3, arcZList3);

  if((ok1 != 0 && ok2 != 0 && ok3 != 0) ||
     (ok1 == 0 && ok2 != 0 && ok3 != 0) ||
     (ok1 != 0 && ok2 == 0 && ok3 != 0) ||
     (ok1 != 0 && ok2 != 0 && ok3 == 0) ||
     (ok1 == 0 && ok2 != 0 && ok3 == 0)){
    //...deletes
    deleteListForCurl(arcZList1, arcZList2, arcZList3);
    return -1;
  }
  
  if(ok1 == 0 && ok2 == 0 && ok3 == 0){
    //...finds
    double checkArc;
    int n1 = arcZList1.length();
    int n2 = arcZList2.length();
    int n3 = arcZList3.length();
    for(int i=0;i<n1;i++){
        for(int j=0;j<n2;j++){
          for(int k=0;k<n3;k++){
            AList<HepPoint3D> arcZ;
            arcZ.append(*arcZList1[i]);
            arcZ.append(*arcZList2[j]);
            arcZ.append(*arcZList3[k]);
            arcZ.sort(TArcOrder);
            double z01 = fabs(arcZ[0]->y()-arcZ[1]->y());
            double z12 = fabs(arcZ[1]->y()-arcZ[2]->y());
            if(z01 <= minZ1 && z12 <= minZ2){
                minZ1 = z01;
                minZ2 = z12;
                flag1 = i;
                flag2 = j;
                flag3 = k;
            }
          }
        }
    }
    if(minZ1 == 9999999.||
         minZ2 == 9999999.){
        deleteListForCurl(arcZList1, arcZList2, arcZList3);
        return -1;
    }
    //...sets the best values
    HepPoint3D tmp1 = *arcZList1[flag1];
    HepPoint3D tmp2 = *arcZList2[flag2];
    HepPoint3D tmp3 = *arcZList3[flag3];
    link1.position(tmp1);
    link2.position(tmp2);
    link3.position(tmp3);
    deleteListForCurl(arcZList1, arcZList2, arcZList3);
    return 0;
  }else if(ok1 == 0 && ok2 == 0 && ok3 != 0){
    int n1 = arcZList1.length();
    int n2 = arcZList2.length();
    for(int i=0;i<n1;i++){
        for(int j=0;j<n2;j++){
          if(fabs(arcZList1[i]->y()-arcZList2[j]->y()) < minZ01){
            minZ01 = fabs(arcZList1[i]->y()-arcZList2[j]->y());
            flag1 = i;
            flag2 = j;
          }
        }
    }
    if(minZ01 == 9999999.){
        deleteListForCurl(arcZList1, arcZList2, arcZList3);
        return -1;
    }
    //...sets the best values
    HepPoint3D tmp1 = *arcZList1[flag1];
    HepPoint3D tmp2 = *arcZList2[flag2];
    link1.position(tmp1);
    link2.position(tmp2);
    deleteListForCurl(arcZList1, arcZList2, arcZList3);
    return 12;
  }else if(ok1 != 0 && ok2 == 0 && ok3 == 0){
    int n2 = arcZList2.length();
    int n3 = arcZList3.length();
    for(int i=0;i<n2;i++){
        for(int j=0;j<n3;j++){
          if(fabs(arcZList2[i]->y()-arcZList3[j]->y()) < minZ12){
            minZ12 = fabs(arcZList2[i]->y()-arcZList3[j]->y());
            flag2 = i;
            flag3 = j;
          }
        }
    }
    if(minZ12 == 9999999.){
        deleteListForCurl(arcZList1, arcZList2, arcZList3);
        return -1;
    }
    //...sets the best values
    HepPoint3D tmp2 = *arcZList2[flag2];
    HepPoint3D tmp3 = *arcZList3[flag3];
    link1.position(tmp2);
    link2.position(tmp3);
    deleteListForCurl(arcZList1, arcZList2, arcZList3);
    return 23;
  }else{
    deleteListForCurl(arcZList1, arcZList2, arcZList3);
    return -1;
  }
}

void
TTrack::deleteListForCurl(AList<HepPoint3D> &l1,
                          AList<HepPoint3D> &l2) const {
    HepAListDeleteAll(l1);
    HepAListDeleteAll(l2);
}

void
TTrack::deleteListForCurl(AList<HepPoint3D> &l1,
                          AList<HepPoint3D> &l2,
                          AList<HepPoint3D> &l3) const {
    HepAListDeleteAll(l1);
    HepAListDeleteAll(l2);
    HepAListDeleteAll(l3);
}
#endif //OLD_STEREO

int
TTrack::stereoHitForCurl(AList<TMLink> & list) const 
{
  if(list.length() == 0)return -1;

  HepPoint3D center = _helix->center();
  HepPoint3D tmp(-999., -999., 0.);
  double r  = fabs(_helix->curv());
  for(unsigned i = 0, size = list.length(); i < size; ++i){
    TMDCWireHit &h = *const_cast<TMDCWireHit*>(list[i]->hit());
    HepVector3D X = 0.5*(h.wire()->forwardPosition() +
                      h.wire()->backwardPosition());
    HepVector3D x     = HepVector3D(X.x(), X.y(), 0.);
    HepVector3D w     = x - center;
    HepVector3D V     = h.wire()->direction();
    HepVector3D v     = HepVector3D(V.x(), V.y(), 0.);
    double   vmag2 = v.mag2();
    double   vmag  = sqrt(vmag2);
    //...temporary
    for(unsigned j = 0; j < 4; ++j)
      list[i]->arcZ(tmp,j);
    
    //...stereo?
    if (vmag == 0.) continue;
   
    double drift = h.drift(WireHitLeft);
    double R[2] = {r + drift, r - drift};
    double wv = w.dot(v);
    double d2[2];
    d2[0] = vmag2*R[0]*R[0] + (wv*wv - vmag2*w.mag2()); //...= v^2*(r^2 - w^2*sin()^2)...outer
    d2[1] = vmag2*R[1]*R[1] + (wv*wv - vmag2*w.mag2()); //...= v^2*(r^2 - w^2*sin()^2)...inner
    
    //...No crossing in R/Phi plane...
    if (d2[0] < 0. && d2[1] < 0.) continue;

    bool ok_inner(true);
    bool ok_outer(true);
    double d[2] = {-1., -1.};
    //...outer
    if(d2[0] >= 0.){
      d[0] = sqrt(d2[0]);
    }else{
      ok_outer = false;
    }
    if(d2[1] >= 0.){
      d[1] = sqrt(d2[1]);
    }else{
      ok_inner = false;
    }
    
    //...Cal. length and z to crossing points...
    double l[2][2];
    double z[2][2];
    //...outer
    if(ok_outer){
      l[0][0] = (- wv + d[0]) / vmag2; //...= (-wvcos()+d)/v/v = (-wcos() + (r^2 - w^2*sin()^2)^0.5)/v
      l[1][0] = (- wv - d[0]) / vmag2; //...= (-wvcos()+d)/v/v = (-wcos() - (r^2 - w^2*sin()^2)^0.5)/v
      z[0][0] = X.z() + l[0][0]*V.z();
      z[1][0] = X.z() + l[1][0]*V.z();
    }
    //...inner
    if(ok_inner){
      l[0][1] = (- wv + d[1]) / vmag2; //...= (-wvcos()+d)/v/v = (-wcos() + (r^2 - w^2*sin()^2)^0.5)/v
      l[1][1] = (- wv - d[1]) / vmag2; //...= (-wvcos()+d)/v/v = (-wcos() - (r^2 - w^2*sin()^2)^0.5)/v
      z[0][1] = X.z() + l[0][1]*V.z();
      z[1][1] = X.z() + l[1][1]*V.z();
    }
    
    //...Cal. xy position of crossing points...
    HepVector3D p[2][2];
#if 1
    HepVector3D tp[2][2];
#endif
    if(ok_outer){
      p[0][0] = x + l[0][0] * v;
      p[1][0] = x + l[1][0] * v;
#if 1
      tp[0][0] = p[0][0];
      tp[1][0] = p[1][0];
#endif
      HepVector3D tmp_pc = p[0][0] - center;
      HepVector3D pc0 = HepVector3D(tmp_pc.x(), tmp_pc.y(), 0.);
      p[0][0] -= drift/pc0.mag()*pc0;
      tmp_pc = p[1][0] - center;
      HepVector3D pc1 = HepVector3D(tmp_pc.x(), tmp_pc.y(), 0.);
      p[1][0] -= drift/pc1.mag()*pc1;
    }
#define JJTEST 0
    if(ok_inner){
      p[0][1] = x + l[0][1] * v;
      p[1][1] = x + l[1][1] * v;
#if JJTEST
      tp[0][1] = p[0][1];
      tp[1][1] = p[1][1];
#endif
      HepVector3D tmp_pc = p[0][1] - center;
      HepVector3D pc0 = HepVector3D(tmp_pc.x(), tmp_pc.y(), 0.);
      p[0][1] += drift/pc0.mag()*pc0;
      tmp_pc = p[1][1] - center;
      HepVector3D pc1 = HepVector3D(tmp_pc.x(), tmp_pc.y(), 0.);
      p[1][1] += drift/pc1.mag()*pc1;
    }

    //...boolean
    bool ok_xy[2][2];
    if(ok_outer){
      ok_xy[0][0] = true;
      ok_xy[1][0] = true;
    }else{
      ok_xy[0][0] = false;
      ok_xy[1][0] = false;
    }
    if(ok_inner){
      ok_xy[0][1] = true;
      ok_xy[1][1] = true;
    }else{
      ok_xy[0][1] = false;
      ok_xy[1][1] = false;
    }
    if(ok_outer){
      if (_charge * (center.x() * p[0][0].y() - center.y() * p[0][0].x())  < 0.)
        ok_xy[0][0] = false;
      if (_charge * (center.x() * p[1][0].y() - center.y() * p[1][0].x())  < 0.)
        ok_xy[1][0] = false;
    }
    if(ok_inner){
      if (_charge * (center.x() * p[0][1].y() - center.y() * p[0][1].x())  < 0.)
        ok_xy[0][1] = false;
      if (_charge * (center.x() * p[1][1].y() - center.y() * p[1][1].x())  < 0.)
        ok_xy[1][1] = false;
    }
    if(!ok_inner && ok_outer && (!ok_xy[0][0]) && (!ok_xy[1][0])){
      continue;
    }
    if(ok_inner && !ok_outer && (!ok_xy[0][1]) && (!ok_xy[1][1])){
      continue;
    }
    
    //...Check z position...
    if(ok_xy[0][0]){
      if (z[0][0] < h.wire()->backwardPosition().z() || 
          z[0][0] > h.wire()->forwardPosition().z()) ok_xy[0][0] = false;
    }
    if(ok_xy[1][0]){
      if (z[1][0] < h.wire()->backwardPosition().z() || 
          z[1][0] > h.wire()->forwardPosition().z()) ok_xy[1][0] = false;
    }
    if(ok_xy[0][1]){
      if (z[0][1] < h.wire()->backwardPosition().z() || 
          z[0][1] > h.wire()->forwardPosition().z()) ok_xy[0][1] = false;
    }
    if(ok_xy[1][1]){
      if (z[1][1] < h.wire()->backwardPosition().z() || 
          z[1][1] > h.wire()->forwardPosition().z()) ok_xy[1][1] = false;
    }
    if ((!ok_xy[0][0]) && (!ok_xy[1][0]) &&
        (!ok_xy[0][1]) && (!ok_xy[1][1])){
      continue;
    }
    double cosdPhi, dPhi;
    unsigned index;
    index = 0;
    if(ok_xy[0][0]){
      //...cal. arc length...
      cosdPhi = - center.dot((p[0][0] - center).unit()) / center.mag();
      if(fabs(cosdPhi) < 1.0){
        dPhi = acos(cosdPhi);
      }else if(cosdPhi >= 1.0){
        dPhi = 0.;
      }else{
        dPhi = M_PI;
      }
      list[i]->arcZ(HepPoint3D(r*dPhi, z[0][0], 0.), index);
      //std::cout << r*dPhi << ", " << z[0][0] << std::endl;
      ++index;
    }
    if(ok_xy[1][0]){
      //...cal. arc length...
      cosdPhi = - center.dot((p[1][0] - center).unit()) / center.mag();
      if(fabs(cosdPhi) < 1.0){
        dPhi = acos(cosdPhi);
      }else if(cosdPhi >= 1.0){
        dPhi = 0.;
      }else{
        dPhi = M_PI;
      }
      list[i]->arcZ(HepPoint3D(r*dPhi, z[1][0], 0.), index);
      //std::cout << r*dPhi << ", " << z[1][0] << std::endl;
      ++index;
    }
    if(ok_xy[0][1]){
      //...cal. arc length...
      cosdPhi = - center.dot((p[0][1] - center).unit()) / center.mag();
      if(fabs(cosdPhi) < 1.0){
        dPhi = acos(cosdPhi);
      }else if(cosdPhi >= 1.0){
        dPhi = 0.;
      }else{
        dPhi = M_PI;
      }
      list[i]->arcZ(HepPoint3D(r*dPhi, z[0][1], 0.), index);
      //std::cout << r*dPhi << ", " << z[0][1] << std::endl;
      ++index;
    }
    if(ok_xy[1][1]){
      //...cal. arc length...
      cosdPhi = - center.dot((p[1][1] - center).unit()) / center.mag();
      if(fabs(cosdPhi) < 1.0){
        dPhi = acos(cosdPhi);
      }else if(cosdPhi >= 1.0){
        dPhi = 0.;
      }else{
        dPhi = M_PI;
      }
      list[i]->arcZ(HepPoint3D(r*dPhi, z[1][1], 0.), index);
      //std::cout << r*dPhi << ", " << z[1][1] << std::endl;
      ++index;
    }
 
#if JJTEST
    double gmaxX = -9999. ,gminX = 9999.;
    double gmaxY = -9999. ,gminY = 9999.;
    FILE *gnuplot, *data;
    double step = 100.;
    double dStep = 2.*M_PI/step;
    if((data = fopen("dat1","w")) != NULL){
      if(ok_xy[0][0]){
        for(int ii=0;ii<step;++ii){
          double X = tp[0][0].x() + drift*cos(dStep*static_cast<double>(ii));
          double Y = tp[0][0].y() + drift*sin(dStep*static_cast<double>(ii));
          fprintf(data,"%lf, %lf\n",X,Y);
          if(gmaxX < X)gmaxX = X;
          if(gminX > X)gminX = X;
          if(gmaxY < Y)gmaxY = Y;
          if(gminY > Y)gminY = Y;
        }
      }
      fclose(data);
    }
    if((data = fopen("dat2","w")) != NULL){
      if(ok_xy[1][0]){
        for(int ii=0;ii<step;++ii){
          double X = tp[1][0].x() + drift*cos(dStep*static_cast<double>(ii));
          double Y = tp[1][0].y() + drift*sin(dStep*static_cast<double>(ii));
          fprintf(data,"%lf, %lf\n",X,Y);
          if(gmaxX < X)gmaxX = X;
          if(gminX > X)gminX = X;
          if(gmaxY < Y)gmaxY = Y;
          if(gminY > Y)gminY = Y;
        }
      }
      fclose(data);
    }
    if((data = fopen("dat3","w")) != NULL){
      if(ok_xy[0][1]){
        for(int ii=0;ii<step;++ii){
          double X = tp[0][1].x() + drift*cos(dStep*static_cast<double>(ii));
          double Y = tp[0][1].y() + drift*sin(dStep*static_cast<double>(ii));
          fprintf(data,"%lf, %lf\n",X,Y);
          if(gmaxX < X)gmaxX = X;
          if(gminX > X)gminX = X;
          if(gmaxY < Y)gmaxY = Y;
          if(gminY > Y)gminY = Y;
        }
      }
      fclose(data);
    }
    if((data = fopen("dat4","w")) != NULL){
      if(ok_xy[1][1]){
        for(int ii=0;ii<step;++ii){
          double X = tp[1][1].x() + drift*cos(dStep*static_cast<double>(ii));
          double Y = tp[1][1].y() + drift*sin(dStep*static_cast<double>(ii));
          fprintf(data,"%lf, %lf\n",X,Y);
          if(gmaxX < X)gmaxX = X;
          if(gminX > X)gminX = X;
          if(gmaxY < Y)gmaxY = Y;
          if(gminY > Y)gminY = Y;
        }
      }
      fclose(data);
    }
    HepVector3D tX = h.wire()->forwardPosition()-h.wire()->backwardPosition();
    HepVector3D tDist(tX.x(), tX.y(), 0.);
    double tD = tDist.mag();
    double vvvM = 1./ v.mag();
    HepVector3D tDire = vvvM*v;
    step = 2.;
    dStep = tD/step;
    if((data = fopen("dat5","w")) != NULL){
      for(int ii=0;ii<step+1;++ii){
        double X = h.wire()->backwardPosition().x()+dStep*static_cast<double>(ii)*tDire.x();
        double Y = h.wire()->backwardPosition().y()+dStep*static_cast<double>(ii)*tDire.y();
        fprintf(data,"%lf, %lf\n",X,Y);
        if(gmaxX < X)gmaxX = X;
        if(gminX > X)gminX = X;
        if(gmaxY < Y)gmaxY = Y;
        if(gminY > Y)gminY = Y;
      }
      fclose(data);
    }
    if((data = fopen("dat6","w")) != NULL){
      double X = h.wire()->backwardPosition().x();
      double Y = h.wire()->backwardPosition().y();
      fprintf(data,"%lf, %lf\n",X,Y);
      if(gmaxX < X)gmaxX = X;
      if(gminX > X)gminX = X;
      if(gmaxY < Y)gmaxY = Y;
      if(gminY > Y)gminY = Y;
      fclose(data);    
    }
    if((data = fopen("dat7","w")) != NULL){
      double X = h.wire()->forwardPosition().x();
      double Y = h.wire()->forwardPosition().y();
      fprintf(data,"%lf, %lf\n",X,Y);
      if(gmaxX < X)gmaxX = X;
      if(gminX > X)gminX = X;
      if(gmaxY < Y)gmaxY = Y;
      if(gminY > Y)gminY = Y;
      fclose(data);  
    }
    step = 300.;
    dStep = 2.*M_PI/step;
    if((data = fopen("dat8","w")) != NULL){
      for(int ii=0;ii<step;++ii){
        double X = center.x() + r*cos(dStep*static_cast<double>(ii));
        double Y = center.y() + r*sin(dStep*static_cast<double>(ii));
        fprintf(data,"%lf, %lf\n",X,Y);
      }
      fclose(data);
    }
    if((data = fopen("dat9","w")) != NULL){
      if(ok_xy[0][0]){
        double X = p[0][0].x();
        double Y = p[0][0].y();
        fprintf(data,"%lf, %lf\n",X,Y);
      }
      fclose(data);  
    }
    if((data = fopen("dat10","w")) != NULL){
      if(ok_xy[1][0]){
        double X = p[1][0].x();
        double Y = p[1][0].y();
        fprintf(data,"%lf, %lf\n",X,Y);
      }
      fclose(data);  
    }
    if((data = fopen("dat11","w")) != NULL){
      if(ok_xy[0][1]){
        double X = p[0][1].x();
        double Y = p[0][1].y();
        fprintf(data,"%lf, %lf\n",X,Y);
      }
      fclose(data);  
    }
    if((data = fopen("dat12","w")) != NULL){
      if(ok_xy[1][1]){
        double X = p[1][1].x();
        double Y = p[1][1].y();
        fprintf(data,"%lf, %lf\n",X,Y);
      }
      fclose(data);  
    }
    std::cout << "Drift Distance = " << drift << ", xy1cm -> z" << V.z()/v.mag() << "cm, xyDist(cm) = " << tD << std::endl;
    if(tX.z()<0.)std::cout << "ERROR : F < B" << std::endl;
    if((gnuplot = popen("gnuplot","w")) != NULL){
      fprintf(gnuplot,"set size 0.721,1.0 \n");
      fprintf(gnuplot,"set xrange [%f:%f] \n",gminX,gmaxX);
      fprintf(gnuplot,"set yrange [%f:%f] \n",gminY,gmaxY);
      fprintf(gnuplot,"plot \"dat1\" with line, \"dat2\" with line, \"dat3\" with line, \"dat4\" with line, \"dat5\" with line, \"dat6\", \"dat7\", \"dat8\" with line, \"dat9\", \"dat10\", \"dat11\", \"dat12\" \n");
      fflush(gnuplot);
      char tmp[8];
      gets(tmp);
      pclose(gnuplot);
    }
#endif
  }
  return 0;
}

#if !(NEW_FIT2D)
int 
TTrack::approach2D(TMLink & l) const {

  //...Setup...
  const TMDCWire & w = * l.wire();
  Vector a = _helix->a();
  double kappa = a[2];
  double phi0  = a[1];
  HepPoint3D xc = _helix->center();
  HepPoint3D xw = w.xyPosition();
  HepPoint3D xt = _helix->x();
  HepVector3D v0, v1;
  v0 = xt - xc;
  v1 = xw - xc;
  //if (_charge > 0.) {
  //v0 *= -1;
  //v1 *= -1;
  //}
  double vCrs = v0.x() * v1.y() - v0.y() * v1.x();
  double vDot = v0.x() * v1.x() + v0.y() * v1.y();
  double dPhi = atan2(vCrs, vDot);

  //...Cal. phi to rotate...
  //double phiWire = atan2(_charge * (xc.y() - xw.y()),
  //                     _charge * (xc.x() - xw.x()));
  //phiWire = (phiWire > 0.) ? phiWire : phiWire + 2. * M_PI;
  //double dPhi = phiWire - phi0;
  
  //...Why this is needed?...
  //if ((_charge >= 0.) && (dPhi > 0.)) dPhi -= 2. * M_PI;
  //else if ((_charge < 0.) && (dPhi < 0.)) dPhi += 2. * M_PI;
  
  //std::cout << _helix->x(dPhi) << " , " << _helix->x(dPhi+0.2) << " , " << _helix->x(dPhi-0.1) << std::endl;
  
  l.positionOnTrack(_helix->x(dPhi));
  HepPoint3D x = xw;
  x.setZ(0.);
  l.positionOnWire(x);
  l.dPhi(dPhi);
  return 0;
}

int
TTrack::dxda2D(const TMLink & link,
               double dPhi,
               Vector & dxda,
               Vector & dyda,
               Vector & dzda) const {
  
  //...Setup...
  const TMDCWire & w = * link.wire();
  Vector a = _helix->a();
  double dRho  = a[0];
  double phi0  = a[1];
  double kappa = a[2];
 // double rho   = Helix::ConstantAlpha / kappa;
 // double rho   = 333.564095 / kappa;
  const double Bz = -1000*m_pmgnIMF->getReferField();
  double rho   = 333.564095/(kappa * Bz);

  //double tanLambda = a[4];
  
  double sinPhi0 = sin(phi0);
  double cosPhi0 = cos(phi0);
  double sinPhi0dPhi = sin(phi0 + dPhi);
  double cosPhi0dPhi = cos(phi0 + dPhi);
  Vector dphida(5);
  
  HepPoint3D d = _helix->center() - w.xyPosition();
  double dmag2 = d.mag2();
  
  dphida[0] = (sinPhi0 * d.x() - cosPhi0 * d.y()) / dmag2;
  dphida[1] = (dRho + rho)    * (cosPhi0 * d.x() + sinPhi0 * d.y())
    / dmag2 - 1.;
  dphida[2] = (- rho / kappa) * (sinPhi0 * d.x() - cosPhi0 * d.y())
    / dmag2;
  dphida[3] = 0.;
  dphida[4] = 0.;
  
  dxda[0] = cosPhi0 + rho * sinPhi0dPhi * dphida[0];
  dxda[1] = - (dRho + rho) * sinPhi0 + rho * sinPhi0dPhi * (1. + dphida[1]);
  dxda[2] = - rho / kappa * (cosPhi0 - cosPhi0dPhi)
    + rho * sinPhi0dPhi * dphida[2];
  dxda[3] = rho * sinPhi0dPhi * dphida[3];
  dxda[4] = rho * sinPhi0dPhi * dphida[4];
  
  dyda[0] = sinPhi0 - rho * cosPhi0dPhi * dphida[0];
  dyda[1] = (dRho + rho) * cosPhi0 - rho * cosPhi0dPhi * (1. + dphida[1]);
  dyda[2] = - rho / kappa * (sinPhi0 - sinPhi0dPhi)
    - rho * cosPhi0dPhi * dphida[2];
  dyda[3] = - rho * cosPhi0dPhi * dphida[3];
  dyda[4] = - rho * cosPhi0dPhi * dphida[4];
  
  dzda[0] = 0.;
  dzda[1] = 0.;
  dzda[2] = 0.;
  dzda[3] = 1.;
  dzda[4] = - rho * dPhi;
  //dzda[0] = - rho * tanLambda * dphida[0];
  //dzda[1] = - rho * tanLambda * dphida[1];
  //dzda[2] = rho / kappa * tanLambda * dPhi - rho * tanLambda * dphida[2];
  //dzda[3] = 1. - rho * tanLambda * dphida[3];
  //dzda[4] = - rho * dPhi - rho * tanLambda * dphida[4];

  return 0;
}
#endif
#if 0
int 
TTrack::svdHitForCurl(AList<TSvdHit>& svdList) const
{
  HepPoint3D center = _helix->center();
  double r  = fabs(_helix->curv());

  for(unsigned i = 0, size = svdList.length(); i < size; ++i){
    HepPoint3D p(svdList[i]->x(), svdList[i]->y(), 0.);
    double cosdPhi = - center.dot((p - center).unit()) / center.mag();
    double dPhi;
    if(fabs(cosdPhi) < 1.0){
      dPhi = acos(cosdPhi);
    }else if(cosdPhi >= 1.0){
      dPhi = 0.;
    }else{
      dPhi = M_PI;
    }
    HepPoint3D tmp(r*dPhi, svdList[i]->z(), 0.);
    svdList[i]->arcZ(tmp);
  }
  return 0;
}
#endif

#if defined(__GNUG__)
int
SortByPt(const TTrack ** a, const TTrack ** b) {
    if ((* a)->pt() < (* b)->pt()) return 1;
    else if
        ((* a)->pt() == (* b)->pt()) return 0;
    else return -1;
}
#else
extern "C" int
SortByPt(const void* av, const void* bv) {
  const TTrack ** a((const TTrack **)av);
  const TTrack ** b((const TTrack **)bv);
    if ((* a)->pt() < (* b)->pt()) return 1;
    else if
        ((* a)->pt() == (* b)->pt()) return 0;
    else return -1;
}
#endif

#if NEW_FIT2D
int
TTrack::fit2D(unsigned ipFlag, double ipDistance, double ipError) {
#ifdef TRKRECO_DEBUG_DETAIL
  std::cout << "    TTrack::fit2D(r-phi) ..." << std::endl;
#endif
  //if(_fitted)return 0;

  //...Check # of hits...

  //std::cout << "# = " << _links.length() << std::endl;
  //...Setup...
  unsigned nTrial(0);
  Vector a(_helix->a());
  double chi2;
  double chi2Old = 1.0e+99;
  Vector dchi2da(3);
  SymMatrix d2chi2d2a(3,0);
  Vector da(5), dxda(3), dyda(3);
  Vector dDda(3);
  const double convergence = 1.0e-5;
  Vector f(3);
  int err = 0;
  double factor = 1.0;
  unsigned usedWireNumber = 0;  

  //...Fitting loop...
  while(nTrial < 100){
    //...Set up...
    chi2 = 0.;
    for (unsigned j=0;j<3;++j) dchi2da[j] = 0.;
    d2chi2d2a = SymMatrix(3, 0);
    usedWireNumber = 0;

    //...Loop with hits...
    unsigned i = 0;
    while (TMLink * l = _links[i++]) {
      if(l->fit2D() == 0)continue;
      const TMDCWireHit &h = *l->hit();
      
      //...Cal. closest points...
      if(approach2D(*l) != 0)continue;
      double dPhi = l->dPhi();
      const HepPoint3D & onTrack = l->positionOnTrack();
      const HepPoint3D & onWire  = l->positionOnWire();
      HepPoint3D onTrack2(onTrack.x(),onTrack.y(),0.);
      HepPoint3D onWire2(onWire.x(),onWire.y(),0.);

      //...Obtain drift distance and its error...
      unsigned leftRight = WireHitRight;
      if (onWire2.cross(onTrack2).z() < 0.) leftRight = WireHitLeft;
      double distance   = h.drift(leftRight);
      double eDistance  = h.dDrift(leftRight);
      double eDistance2 = eDistance * eDistance;
      if(eDistance == 0.){
        std::cout << "Error(?) : Drift Distance Error = 0 in fit2D of TrkReco." << std::endl;
        continue;
      }

      //...Residual...
      HepVector3D v = onTrack2 - onWire2;
      double vmag = v.mag();
      double dDistance = vmag - distance;

      //...dxda...
      if(this->dxda2D(*l, dPhi, dxda, dyda) != 0)continue;

      //...Chi2 related...
      //Vector3 vw(0.,0.,1.);
      dDda = (vmag > 0.)
        ? (v.x()*dxda+v.y()*dyda)/vmag
        : Vector(3,0);
      if(vmag<=0.0){
        std::cout << "    in fit2D " << onTrack << ", " << onWire;
        h.dump();
        continue;
      }
      dchi2da     += (dDistance/eDistance2)*dDda;
      d2chi2d2a   += vT_times_v(dDda)/eDistance2;
      double pChi2 = dDistance*dDistance/eDistance2;
      chi2        += pChi2;
      
      //...Store results...
      l->update(onTrack2, onWire2, leftRight, pChi2);
      ++usedWireNumber;
    }
    if(ipFlag != 0){
      double kappa = _helix->a()[2];
      double phi0  = _helix->a()[1];
      HepPoint3D xc(_helix->center());
      HepPoint3D onWire(0.,0.,0.);
      HepPoint3D xt(_helix->x());
      HepVector3D v0(xt-xc);
      HepVector3D v1(onWire-xc);
      double vCrs = v0.x() * v1.y() - v0.y() * v1.x();
      double vDot = v0.x() * v1.x() + v0.y() * v1.y();
      double dPhi = atan2(vCrs, vDot);      
      HepPoint3D onTrack(_helix->x(dPhi).x(),_helix->x(dPhi).y(),0.);
      double distance   = ipDistance;
      double eDistance  = ipError;
      double eDistance2 = eDistance * eDistance;

      HepVector3D v = onTrack - onWire;
      double vmag = v.mag();
      double dDistance = vmag - distance;

      if(this->dxda2D(dPhi, dxda, dyda) != 0)goto ipOff;
      
      dDda = (vmag > 0.)
        ? (v.x()*dxda+v.y()*dyda)/vmag
        : Vector(3,0);
      if(vmag<=0.0){
        goto ipOff;
      }
      dchi2da     += (dDistance/eDistance2)*dDda;
      d2chi2d2a   += vT_times_v(dDda)/eDistance2;
      double pChi2 = dDistance*dDistance/eDistance2;
      chi2        += pChi2;
      
      ++usedWireNumber;
    }
  ipOff:
    if(usedWireNumber < 4){
      err = -2;
      break;
    }

    //...Check condition...
    double change = chi2Old - chi2;
    if(fabs(change) < convergence)break;
    if(change < 0.){
#ifdef TRKRECO_DEBUG_DETAIL
      std::cout << "chi2Old, chi2=" << chi2Old <<" "<< chi2 << std::endl;
#endif
      //change to the old value.
      a += factor*da;
      _helix->a(a);

      chi2 = 0.;
      for (unsigned j=0;j<3;++j) dchi2da[j] = 0.;
      d2chi2d2a = SymMatrix(3,0);
      usedWireNumber = 0;
   
      //...Loop with hits...
      unsigned i = 0;
      while (TMLink *l = _links[i++]) {
        if(l->fit2D() == 0)continue;
        const TMDCWireHit & h = * l->hit();
        
        //...Cal. closest points...
        if(approach2D(*l) != 0)continue;
        double dPhi = l->dPhi();
        const HepPoint3D & onTrack = l->positionOnTrack();
        const HepPoint3D & onWire  = l->positionOnWire();
        HepPoint3D onTrack2(onTrack.x(),onTrack.y(),0.);
        HepPoint3D onWire2(onWire.x(),onWire.y(),0.);
        
        //...Obtain drift distance and its error...
        unsigned leftRight = WireHitRight;
        if (onWire2.cross(onTrack2).z() < 0.) leftRight = WireHitLeft;
        double distance   = h.drift(leftRight);
        double eDistance  = h.dDrift(leftRight);
        double eDistance2 = eDistance * eDistance;
            
        //...Residual...
        HepVector3D v = onTrack2 - onWire2;
        double vmag = v.mag();
        double dDistance = vmag - distance;

        //...dxda...
        if(this->dxda2D(*l, dPhi, dxda, dyda) != 0)continue;

        //...Chi2 related...
        dDda = (vmag>0.)
          ? (v.x()*dxda + v.y()*dyda)/vmag
          : Vector(3,0);
        if(vmag<=0.0) {
          std::cout << "    in fit2D " << onTrack << ", " << onWire;
          h.dump();
          continue;
        }
        dchi2da     += (dDistance/eDistance2)*dDda;
        d2chi2d2a   += vT_times_v(dDda)/eDistance2;
        double pChi2 = dDistance*dDistance/eDistance2;
        chi2        += pChi2;
        
        //...Store results...
        l->update(onTrack2, onWire2, leftRight, pChi2);
        ++usedWireNumber;
      }
      if(ipFlag != 0){
        double kappa = _helix->a()[2];
        double phi0  = _helix->a()[1];
        HepPoint3D xc(_helix->center());
        HepPoint3D onWire(0.,0.,0.);
        HepPoint3D xt(_helix->x());
        HepVector3D v0(xt-xc);
        HepVector3D v1(onWire-xc);
        double vCrs = v0.x() * v1.y() - v0.y() * v1.x();
        double vDot = v0.x() * v1.x() + v0.y() * v1.y();
        double dPhi = atan2(vCrs, vDot);      
        HepPoint3D onTrack(_helix->x(dPhi).x(),_helix->x(dPhi).y(),0.);
        double distance   = ipDistance;
        double eDistance  = ipError;
        double eDistance2 = eDistance * eDistance;
        
        HepVector3D v = onTrack - onWire;
        double vmag = v.mag();
        double dDistance = vmag - distance;
        
        if(this->dxda2D(dPhi, dxda, dyda) != 0)goto ipOff2;
        
        dDda = (vmag > 0.)
          ? (v.x()*dxda+v.y()*dyda)/vmag
          : Vector(3,0);
        if(vmag<=0.0){
          goto ipOff2;
        }
        dchi2da     += (dDistance/eDistance2)*dDda;
        d2chi2d2a   += vT_times_v(dDda)/eDistance2;
        double pChi2 = dDistance*dDistance/eDistance2;
        chi2        += pChi2;
        
        ++usedWireNumber;
      }
    ipOff2:
      if(usedWireNumber < 4){
        err = -2;
        break;
      }
      //break;
      factor *= 0.75;
#ifdef TRKRECO_DEBUG_DETAIL 
      std::cout << "factor = " << factor << std::endl;
      std::cout << "chi2 = " << chi2 << std::endl;
#endif
      if(factor < 0.01)break;
    }

    chi2Old = chi2;

    //...Cal. helix parameters for next loop...
    f = solve(d2chi2d2a, dchi2da);
    da[0] = f[0];
    da[1] = f[1];
    da[2] = f[2];
    da[3] = 0.;
    da[4] = 0.;
    
    a -= factor*da;
    _helix->a(a);      
    ++nTrial;
  }
  if(err){
    return err;
  }

  //...Cal. error matrix...
  SymMatrix Ea(5,0);
  unsigned dim = 3;
  SymMatrix Eb = d2chi2d2a;
  SymMatrix Ec = Eb.inverse(err);
  Ea[0][0] = Ec[0][0];
  Ea[0][1] = Ec[0][1];
  Ea[0][2] = Ec[0][2];
  Ea[1][1] = Ec[1][1];
  Ea[1][2] = Ec[1][2];
  Ea[2][2] = Ec[2][2];

  //...Store information...
  if(!err){
    _helix->a(a);
    _helix->Ea(Ea);
  }else{
    err = -2;
  }
    
  _ndf  = usedWireNumber-dim;
  _chi2 = chi2;
    
  //_fitted = true;

#define JJJTEST 0
#if JJJTEST
  double gmaxX = -9999. ,gminX = 9999.;
  double gmaxY = -9999. ,gminY = 9999.;
  FILE *gnuplot, *data;
  double step = 200.;
  double dStep = 2.*M_PI/step;
  for(int i=0,size = _links.length();i<size;++i){
    TMLink * l = _links[i];
    double drift = l->hit()->distance(0);
    char name[100] = "dat";
    char counter[10] = "";
    sprintf(counter,"%02d",i);
    strcat(name,counter);
    if((data = fopen(name,"w")) != NULL){
      for(int ii=0;ii<step;++ii){
        double X = l->wire()->xyPosition().x() + drift*cos(dStep*static_cast<double>(ii));
        double Y = l->wire()->xyPosition().y() + drift*sin(dStep*static_cast<double>(ii));
        fprintf(data,"%lf, %lf\n",X,Y);
        if(gmaxX < X)gmaxX = X;
        if(gminX > X)gminX = X;
        if(gmaxY < Y)gmaxY = Y;
        if(gminY > Y)gminY = Y;
      }
      fclose(data);
    }
  }
  step = 300.;
  dStep = 2.*M_PI/step;
  if((data = fopen("datc","w")) != NULL){
    for(int ii=0;ii<step;++ii){
      double X = _helix->center().x() + _helix->radius()*cos(dStep*static_cast<double>(ii));
      double Y = _helix->center().y() + _helix->radius()*sin(dStep*static_cast<double>(ii));
      fprintf(data,"%lf, %lf\n",X,Y);
    }
    fclose(data);
  }
  if((gnuplot = popen("gnuplot","w")) != NULL){
    fprintf(gnuplot,"set size 0.721,1.0 \n");
    fprintf(gnuplot,"set xrange [%f:%f] \n",gminX,gmaxX);
    fprintf(gnuplot,"set yrange [%f:%f] \n",gminY,gmaxY);
    if(_links.length() == 4){
      fprintf(gnuplot,"plot \"datc\" with line, \"dat00\" with line, \"dat01\" with line, \"dat02\" with line, \"dat03\" with line \n");
    }else if(_links.length() == 5){
      fprintf(gnuplot,"plot \"datc\" with line, \"dat00\" with line, \"dat01\" with line, \"dat02\" with line, \"dat03\" with line, \"dat04\" with line \n");
    }else if(_links.length() == 6){
      fprintf(gnuplot,"plot \"datc\" with line, \"dat00\" with line, \"dat01\" with line, \"dat02\" with line, \"dat03\" with line, \"dat04\" with line, \"dat05\" with line \n");
    }else if(_links.length() == 7){
      fprintf(gnuplot,"plot \"datc\" with line, \"dat00\" with line, \"dat01\" with line, \"dat02\" with line, \"dat03\" with line, \"dat04\" with line, \"dat05\" with line, \"dat06\" with line \n");
    }else if(_links.length() == 8){
      fprintf(gnuplot,"plot \"datc\" with line, \"dat00\" with line, \"dat01\" with line, \"dat02\" with line, \"dat03\" with line, \"dat04\" with line, \"dat05\" with line, \"dat06\" with line, \"dat07\" with line \n");
    }else if(_links.length() == 9){
      fprintf(gnuplot,"plot \"datc\" with line, \"dat00\" with line, \"dat01\" with line, \"dat02\" with line, \"dat03\" with line, \"dat04\" with line, \"dat05\" with line, \"dat06\" with line, \"dat07\" with line, \"dat08\" with line \n");
    }else if(_links.length() == 10){
      fprintf(gnuplot,"plot \"datc\" with line, \"dat00\" with line, \"dat01\" with line, \"dat02\" with line, \"dat03\" with line, \"dat04\" with line, \"dat05\" with line, \"dat06\" with line, \"dat07\" with line, \"dat08\" with line, \"dat09\" with line \n");
    }else if(_links.length() >= 11){
      fprintf(gnuplot,"plot \"datc\" with line, \"dat00\" with line, \"dat01\" with line, \"dat02\" with line, \"dat03\" with line, \"dat04\" with line, \"dat05\" with line, \"dat06\" with line, \"dat07\" with line, \"dat08\" with line, \"dat09\" with line, \"dat10\" with line \n");
    }
    fflush(gnuplot);
    char tmp[8];
    gets(tmp);
    pclose(gnuplot);
  }
#endif //JJJTEST

  return err;
}

int 
TTrack::approach2D(TMLink &l) const {

  const TMDCWire &w = *l.wire();
  double kappa = _helix->a()[2];
  double phi0  = _helix->a()[1];
  HepPoint3D xc(_helix->center());
  HepPoint3D xw(w.xyPosition());
  HepPoint3D xt(_helix->x());
  HepVector3D v0(xt-xc);
  HepVector3D v1(xw-xc);

  double vCrs = v0.x() * v1.y() - v0.y() * v1.x();
  double vDot = v0.x() * v1.x() + v0.y() * v1.y();
  double dPhi = atan2(vCrs, vDot);

  xt = _helix->x(dPhi);
  xt.setZ(0.);
  l.positionOnTrack(xt);
  xw.setZ(0.);
  l.positionOnWire(xw);
  l.dPhi(dPhi);
  return 0;
}

int
TTrack::dxda2D(const TMLink & link,
               double dPhi,
               Vector & dxda,
               Vector & dyda) const {
  
  //...Setup...
  double kappa = (_helix->a())[2];
  if(kappa == 0.){
    std::cout << "Error(?) : kappa == 0 in dxda2D of TrkReco." << std::endl;
    return 1;
  }
  const TMDCWire &w = *link.wire();
  double dRho  = (_helix->a())[0];
  double phi0  = (_helix->a())[1];
 // double rho   = Helix::ConstantAlpha / kappa;
 // double rho   = 333.564095 / kappa;
  const double Bz = -1000*m_pmgnIMF->getReferField();
  double rho   = 333.564095/(kappa * Bz);  

  double sinPhi0 = sin(phi0);
  double cosPhi0 = cos(phi0);
  double sinPhi0dPhi = sin(phi0 + dPhi);
  double cosPhi0dPhi = cos(phi0 + dPhi);
  Vector dphida(3);
  
  HepPoint3D d = _helix->center() - w.xyPosition();
  d.setZ(0.);
  double dmag2 = d.mag2();
  
  if(dmag2 == 0.){
    std::cout << "Error(?) : Distance(center-xyPosition) == 0 in dxda2D of TrkReco." << std::endl;
    return 1;
  }

  dphida[0] = (sinPhi0*d.x()-cosPhi0*d.y())/dmag2;
  dphida[1] = (dRho+rho)*(cosPhi0*d.x()+sinPhi0 * d.y())/dmag2 - 1.;
  dphida[2] = (-rho/kappa)*(sinPhi0*d.x()-cosPhi0*d.y())/dmag2;
  
  dxda[0] = cosPhi0+rho*sinPhi0dPhi*dphida[0];
  dxda[1] = -(dRho+rho)*sinPhi0+rho*sinPhi0dPhi*(1.+dphida[1]);
  dxda[2] = -rho/kappa*(cosPhi0-cosPhi0dPhi)+rho*sinPhi0dPhi*dphida[2];

  dyda[0] = sinPhi0-rho*cosPhi0dPhi*dphida[0];
  dyda[1] = (dRho+rho)*cosPhi0-rho*cosPhi0dPhi*(1.+dphida[1]);
  dyda[2] = -rho/kappa*(sinPhi0-sinPhi0dPhi)-rho*cosPhi0dPhi*dphida[2];
  
  return 0;
}

int
TTrack::dxda2D(double dPhi,
               Vector & dxda,
               Vector & dyda) const {
  
  //...Setup...
  double kappa = (_helix->a())[2];
  if(kappa == 0.){
    std::cout << "Error(?) : kappa == 0 in dxda2D of TrkReco." << std::endl;
    return 1;
  }
  double dRho  = (_helix->a())[0];
  double phi0  = (_helix->a())[1];
 // double rho   = Helix::ConstantAlpha / kappa;
 // double rho   = 333.564095 / kappa;  
  const double Bz = -1000*m_pmgnIMF->getReferField();
  double rho   = 333.564095/(kappa * Bz); 

  double sinPhi0 = sin(phi0);
  double cosPhi0 = cos(phi0);
  double sinPhi0dPhi = sin(phi0 + dPhi);
  double cosPhi0dPhi = cos(phi0 + dPhi);
  Vector dphida(3);
  
  HepPoint3D d = _helix->center(); // d = center - (0,0,0)
  d.setZ(0.);
  double dmag2 = d.mag2();
  
  if(dmag2 == 0.){
    std::cout << "Error(?) : Distance(center-xyPosition) == 0 in dxda2D of TrkReco." << std::endl;
    return 1;
  }

  dphida[0] = (sinPhi0*d.x()-cosPhi0*d.y())/dmag2;
  dphida[1] = (dRho+rho)*(cosPhi0*d.x()+sinPhi0 * d.y())/dmag2 - 1.;
  dphida[2] = (-rho/kappa)*(sinPhi0*d.x()-cosPhi0*d.y())/dmag2;
  
  dxda[0] = cosPhi0+rho*sinPhi0dPhi*dphida[0];
  dxda[1] = -(dRho+rho)*sinPhi0+rho*sinPhi0dPhi*(1.+dphida[1]);
  dxda[2] = -rho/kappa*(cosPhi0-cosPhi0dPhi)+rho*sinPhi0dPhi*dphida[2];

  dyda[0] = sinPhi0-rho*cosPhi0dPhi*dphida[0];
  dyda[1] = (dRho+rho)*cosPhi0-rho*cosPhi0dPhi*(1.+dphida[1]);
  dyda[2] = -rho/kappa*(sinPhi0-sinPhi0dPhi)-rho*cosPhi0dPhi*dphida[2];
  
  return 0;
}
#endif

unsigned
TTrack::defineType(void) const {
    bool highPt = true;
    if (pt() < 0.2) highPt = false;     //Bes
    bool fromIP = true;
    if (fabs(impact()) > 8.3) fromIP = false;

    if (fromIP && highPt) return _type = TrackTypeNormal;
    else if (fromIP && (! highPt)) return _type = TrackTypeCurl;

    if ((fabs(radius()) * 2. + fabs(impact())) < 81.)  //Bes
        return _type = TrackTypeCircle;
    return _type = TrackTypeCosmic;
}

std::string
TrackType(unsigned type) {
    switch (type) {
    case TrackTypeUndefined:
        return std::string("undefined");
    case TrackTypeNormal:
        return std::string("normal");
    case TrackTypeCurl:
        return std::string("curl  ");
    case TrackTypeCircle:
        return std::string("circle");
    case TrackTypeCosmic:
        return std::string("cosmic");
    }
    return std::string("unknown  ");
}

#ifdef OPTNK
#define t_dot(a,b) (a[0]*b[0]+a[1]*b[1]+a[2]*b[2])
#define t_dot2(a,b) (a[0]*b[0]+a[1]*b[1])
#define t_print(a,b) std::cout << b << " = " << a[0] << " " << a[1] << " " << a[2] << std::endl;
#endif

//#define TRKRECO_DEBUG
#ifdef TRKRECO_DEBUG
//#include "tuple/BelleTupleManager.h"
//BelleHistogram * h_nTrial;
#endif

int 
TTrack::approach(TMLink & link, bool doSagCorrection) const {
    //...Cal. dPhi to rotate...
    const TMDCWire & w = * link.wire();
    double wp[3]; w.xyPosition(wp);
    double wb[3]; w.backwardPosition(wb);
    double v[3];
    v[0] = w.direction().x();
    v[1] = w.direction().y();
    v[2] = w.direction().z();
    //...Sag correction...
    if (doSagCorrection) {
        HepVector3D dir = w.direction();
        HepPoint3D xw(wp[0], wp[1], wp[2]);
        HepPoint3D wireBackwardPosition(wb[0], wb[1], wb[2]);
        w.wirePosition(link.positionOnTrack().z(),
                       xw,
                       wireBackwardPosition,
                       dir);
        v[0] = dir.x();
        v[1] = dir.y();
        v[2] = dir.z();
        wp[0] = xw.x();
        wp[1] = xw.y();
        wp[2] = xw.z();
        wb[0] = wireBackwardPosition.x();
        wb[1] = wireBackwardPosition.y();
        wb[2] = wireBackwardPosition.z();
    }
    //...Cal. dPhi to rotate...
    const HepPoint3D & xc = _helix->center();
    double xt[3]; _helix->x(0., xt);
    double x0 = - xc.x();
    double y0 = - xc.y();
    double x1 = wp[0] + x0;
    double y1 = wp[1] + y0;
    x0 += xt[0];
    y0 += xt[1];
    double dPhi = atan2(x0 * y1 - y0 * x1, x0 * x1 + y0 * y1);
    //...Setup...
    double kappa = _helix->kappa();
    double phi0 = _helix->phi0();
//yzhang 2012-08-30
    //...Axial case...
//    if (w.axial()) {
//      link.positionOnTrack(_helix->x(dPhi));
//      HepPoint3D x(wp[0], wp[1], wp[2]);
//      x.setZ(link.positionOnTrack().z());
//      link.positionOnWire(x);
//      link.dPhi(dPhi);
//      return 0;
//    }
#ifdef TRKRECO_DEBUG
    double firstdfdphi = 0.;
    static bool first = true;
    if (first) {
//      extern BelleTupleManager * BASF_Histogram;
//      BelleTupleManager * m = BASF_Histogram;
//      h_nTrial = m->histogram("TTrack::approach nTrial", 100, 0., 100.);
    }
#endif

    //...Stereo case...
    // double rho = Helix::ConstantAlpha / kappa;
    // double rho = 333.564095 / kappa;
    // yzhang 2012-05-03 delete
    //double rho = 333.564095/  kappa;
    //yzhang add 
    Gaudi::svcLocator()->service("MagneticFieldSvc",m_pmgnIMF);
    if(m_pmgnIMF==NULL) {
      std::cout<< __FILE__<<" "<<__LINE__<<" Unable to open Magnetic field service"<<std::endl;
    }
    const double Bz = -1000*m_pmgnIMF->getReferField();
    double rho   = 333.564095/(kappa * Bz); 
    //zhangy
    double tanLambda = _helix->tanl();
    static Vector x(3);
    double t_x[3];
    double t_dXdPhi[3];
    const double convergence = 1.0e-5;
    double l;
    unsigned nTrial = 0;
    while (nTrial < 100) {

        x = link.positionOnTrack(_helix->x(dPhi));
        t_x[0] = x[0];
        t_x[1] = x[1];
        t_x[2] = x[2];

        l = v[0] * t_x[0] + v[1] * t_x[1] + v[2] * t_x[2]
            - v[0] * wb[0] - v[1] * wb[1] - v[2] * wb[2];

        double rcosPhi = rho * cos(phi0 + dPhi);
        double rsinPhi = rho * sin(phi0 + dPhi);
        t_dXdPhi[0] =   rsinPhi;
        t_dXdPhi[1] = - rcosPhi;
        t_dXdPhi[2] = - rho * tanLambda;

        //...f = d(Distance) / d phi...
        double t_d2Xd2Phi[2];
        t_d2Xd2Phi[0] = rcosPhi;
        t_d2Xd2Phi[1] = rsinPhi;

        //...iw new...
        double n[3];
        n[0] = t_x[0] - wb[0];
        n[1] = t_x[1] - wb[1];
        n[2] = t_x[2] - wb[2];
        
        double a[3];
        a[0] = n[0] - l * v[0];
        a[1] = n[1] - l * v[1];
        a[2] = n[2] - l * v[2];
        double dfdphi = a[0] * t_dXdPhi[0]
            + a[1] * t_dXdPhi[1]
            + a[2] * t_dXdPhi[2];

#ifdef TRKRECO_DEBUG
        if (nTrial == 0) {
//          break;
            firstdfdphi = dfdphi;
        }

        //...Check bad case...
        if (nTrial > 3) {
            std::cout << "TTrack::approach ... " << w.name() << " "
                      << "dfdphi(0)=" << firstdfdphi
                      << ",(" << nTrial << ")=" << dfdphi << endl;
        }
#endif

        //...Is it converged?...
        if (fabs(dfdphi) < convergence)
            break;

        double dv = v[0] * t_dXdPhi[0]
            + v[1] * t_dXdPhi[1]
            + v[2] * t_dXdPhi[2];
        double t0 = t_dXdPhi[0] * t_dXdPhi[0]
            + t_dXdPhi[1] * t_dXdPhi[1]
            + t_dXdPhi[2] * t_dXdPhi[2];
        double d2fd2phi = t0 - dv * dv
            + a[0] * t_d2Xd2Phi[0]
            + a[1] * t_d2Xd2Phi[1];
//          + a[2] * t_d2Xd2Phi[2];

        dPhi -= dfdphi / d2fd2phi;

//      cout << "nTrial=" << nTrial << endl;
//      cout << "iw f,df,dphi=" << dfdphi << "," << d2fd2phi << "," << dPhi << endl;

        ++nTrial;
    }

    //...Cal. positions...
    link.positionOnWire(HepPoint3D(wb[0] + l * v[0],
                                wb[1] + l * v[1],
                                wb[2] + l * v[2]));
    link.dPhi(dPhi);

#ifdef TRKRECO_DEBUG
//    h_nTrial->accumulate((float) nTrial + .5);
#endif

    return nTrial;
}

/*
void 
TTrack::relationClusterWithWire(){
//----------------------------------------------------------
// Note: Selection of associating cluster to TMLink is done.
//       if appropriate cluster was found ,
//       relation to cluster is set to the TMLink.
//----------------------------------------------------------

  int j = 0;
  while( TMLink *l = _links[j] ) {

  // initialization
   int flag(-1);
   l->setusecathode(0);
   double dPhi = l->dPhi();
   l->setZphiBeforeCathode( _helix->x(dPhi).z() );

   int k1  = 0;
   while( TMDCCatHit *c = _catHits[k1] ){

   // Matching of layer 
    if( c->layerID() != l->hit()->wire()->layerId() ) {
       k1++; continue;
       }

   // Matching of wire hit 
    AList<TMDCWireHit>& cwire = c->candwire();
    AList<TMDCClust>&  cclust = c->candclust();

    if( cwire.length() == 0 || cclust.length() == 0 ){
        k1++; continue;
       }

    int k2 = 0;
    while( TMDCWireHit *cw = cwire[k2] ){
      if( cw == l->hit() ){  flag = 1; break; }
        k2++;
     }

    if( flag == -1 ){
       k1++; continue;
    }

   // Selection of cluster
    flag = -1;
    float cand_sector = CathodeSectorId( cwire[k2]->wire()->id());

    // --- debug ---
    //      std::cout << "cand_sector = " << cand_sector << std::endl;
    //--- debug ---

    cand_sector = cand_sector - (int(cand_sector/8))*8;

    // --- debug ---
    // std::cout << "cand_sector(local) = " << cand_sector << std::endl;
    // --- debug ---

    int count_at_boundary(0);
    float tmaxpad_at_boundary(0.);
    TMDCClust * cc_at_boundary = NULL;

    k2 = 0;
    while( TMDCClust *cc = cclust[k2] ){

      unsigned cathit_sector = cc->maxpad()->sectorId();
      
    if( cand_sector == float(cathit_sector) ) {
      l->setusecathode(1);  l->setmclust(cc); break ;
    } else if( abs( cand_sector - float(cathit_sector)) == 0.5 ) {

        count_at_boundary += 1;
        l->setusecathode(1);
        l->setmclust(cc);

        if( count_at_boundary == 1 ) {
              cc_at_boundary = cc;
              tmaxpad_at_boundary = cc->tmaxpad();
        }

        //.. if 2 candidates exist at sector boundary,
        //   choose the best matching of T between cluster and wire 
        if( count_at_boundary == 2 ) {
          if( fabs( cc->tmaxpad() - l->hit()->reccdc()->m_tdc )
            > fabs( tmaxpad_at_boundary - l->hit()->reccdc()->m_tdc ) )
              l->setmclust(cc_at_boundary);
          break;
         }

    }

         k2++;
        }
    k1++;
    } // TMDCCatHit loop

   j++;
   } // TMLink loop

}

// addition by matsu ( 1999/07/05 )
void TTrack::relationClusterWithLayer( int SysCorr ){

    //... selection of cathode cluster
    for(unsigned it0=0;it0<3;it0++){
    
      AList<TMLink> catLink = SameLayer( cores(), it0 );

      unsigned n(catLink.length());
      if( !n ) continue;

      int BestID(-1);

      double tmpz(DBL_MAX);
      for(unsigned it1=0;it1<n;it1++){
        TMLink & l = * catLink[it1];

       if( l.getmclust() && l.usecathode() != 0 ){

          l.setusecathode(2);

          double Zdiff(l.positionOnTrack().z());
          if(SysCorr ) Zdiff -=  l.getmclust()->zclustWithSysCorr();
          else         Zdiff -=  l.getmclust()->zclust();
          if( fabs(Zdiff) < tmpz ){
          tmpz = fabs(Zdiff);  BestID = it1;
          }
       }
      }

   if( BestID == -1 ) continue;

     for(unsigned it1=0;it1<n;it1++){
     TMLink & l = * catLink[it1];
     if( l.getmclust() && l.usecathode() != 0 ){
        if( it1 == BestID ) {
          l.setusecathode(3); break;
        }
       }
     }
   }
}
*/


int
TTrack::szPosition(TMLink & link) const {
    const TMDCWireHit & h = * link.hit();
    HepVector3D X = 0.5 * (h.wire()->forwardPosition()
                        + h.wire()->backwardPosition());
    //    double theta = atan2(X.y(), X.x());
    //    HepVector3D lr(h.distance(WireHitLeft) * sin(theta),
    //          - h.distance(WireHitLeft) * cos(theta),
    //          0.);

    HepVector3D xx = HepVector3D(X.x(), X.y(), 0.);
    HepPoint3D center = _helix->center();
    HepVector3D yy = center - xx;
    HepVector3D ww = HepVector3D(yy.x(), yy.y(), 0.);
    double wwmag2 = ww.mag2();
    double wwmag = sqrt(wwmag2);
    HepVector3D lr(h.drift(WireHitLeft)/wwmag * ww.x(),
                h.drift(WireHitLeft)/wwmag * ww.y(),
                0.);
 
#ifdef TRKRECO_DEBUG_DETAIL
    std::cout<<"old lr "<<lr<<endl;
    std::cout<<"old X "<<X<<endl;
    std::cout<<"link.leftRight "<<link.leftRight()<<endl;
#endif
    //...Check left or right...
    //   // change to bes3 .. test..
    //if (link.leftRight() == WireHitRight) {
    //lr = - lr;
    //}
    //if (link.leftRight() == WireHitLeft) lr = - lr; 
    //else if (link.leftRight() == 2) lr = ORIGIN;

    //yzhang 2012-05-07 
    const double Bz = -1000*m_pmgnIMF->getReferField();
#ifdef TRKRECO_DEBUG_DETAIL
    std::cout<<"charge "<<_charge<<" Bz "<<Bz<<endl;
#endif
    if (_charge*Bz > 0){ //yzhang 2012-05-07 
      if (link.leftRight() == WireHitRight){
        lr = - lr;//right
      }
    }else{
      if (link.leftRight() == WireHitLeft){
        lr = - lr;//left
      }
    }
    if (link.leftRight() == 2) lr = ORIGIN;
    //zhangy

    X += lr;

    //...Prepare vectors...
    //    HepPoint3D center = _helix->center();
    HepPoint3D tmp(-9999., -9999., 0.);
    HepVector3D x = HepVector3D(X.x(), X.y(), 0.);
    HepVector3D w = x - center;
    // //modified the next sentence because the direction are different from belle.
    HepVector3D V = h.wire()->direction();
    //    //    to bes3
    // //    HepVector3D V = - h.wire()->direction();   

    HepVector3D v = HepVector3D(V.x(), V.y(), 0.);
    double vmag2 = v.mag2();
    double vmag = sqrt(vmag2);
    
    double r = _helix->curv();
    double wv = w.dot(v);
    //    //zsl for bes3
    //    wv = abs(wv);
    double d2 = wv * wv - vmag2 * (w.mag2() - r * r);
#ifdef TRKRECO_DEBUG_DETAIL
    std::cout<<"lr "<<lr<<endl;
    std::cout<<"forwardPosition "<<h.wire()->forwardPosition()<<endl;
    std::cout<<"backwardPosition "<<h.wire()->backwardPosition()<<endl;
    std::cout<<"X "<<X<<endl;
    std::cout<<"center "<<center<<endl;
    std::cout<<"xx "<<xx<<endl;
    std::cout<<"ww "<<ww<<endl;
    std::cout<<"TTrack::wire direction:"<<h.wire()->direction()<<endl;
    std::cout<<"x "<<x<<endl;
    std::cout<<"w "<<w<<endl;
    std::cout<<"sz,Track::vmag:"<<vmag<<", helix_r:"<<r<<", wv:"<<wv<<", d:"<<sqrt(d2)<<endl;     
#endif

     //...No crossing in R/Phi plane... This is too tight...

    if (d2 < 0.) {
        link.position(tmp);

#ifdef TRKRECO_DEBUG
        std::cout << "TTrack !!! stereo: 0. > d2 = " << d2 << " "
                  << link.leftRight() << std::endl;
#endif
        return -1;
    }
    double d = sqrt(d2);

    //...Cal. length to crossing points...
    double l[2];
    l[0] = (- wv + d) / vmag2;
    l[1] = (- wv - d) / vmag2;

    //...Cal. z of crossing points...
    bool ok[2];
    ok[0] = true;
    ok[1] = true;
    double z[2];
    z[0] = X.z() + l[0] * V.z();
    z[1] = X.z() + l[1] * V.z();
#ifdef TRKRECO_DEBUG_DETAIL
    std::cout<<"X.z():"<<X.z()<<endl;
    std::cout<<"szPosition::z(0) "<<z[0]<<" z(1)"<<z[1]<<" leftRight "<<link.leftRight()<<endl;
    std::cout<<"szPosition::wire backwardPosition and forwardPosition:"<< h.wire()->backwardPosition().z()<<","<<h.wire()->forwardPosition().z()<<endl;
    std::cout << "    l0, l1 = " << l[0] << ", " << l[1] << std::endl;
    std::cout << "    z0, z1 = " << z[0] << ", " << z[1] << std::endl;
    std::cout << "    backward = " << h.wire()->backwardPosition().z() << std::endl;
    std::cout << "    forward = " << h.wire()->forwardPosition().z() << std::endl;
#endif

    //...Check z position... //yzhang change 2012-05-03 
    //modified because Belle backward and forward are different from BESIII
    
    /*
    if (link.leftRight() == 2) {
        if (z[0] > h.wire()->backwardPosition().z()+20. 
            || z[0] < h.wire()->forwardPosition().z()-20.) ok[0] = false;
        if (z[1] > h.wire()->backwardPosition().z()+20.
            || z[1] < h.wire()->forwardPosition().z()-20.) ok[1] = false;
    }
    else {
        if (z[0] > h.wire()->backwardPosition().z()
            || z[0] < h.wire()->forwardPosition().z() ) ok[0] = false;
        if (z[1] > h.wire()->backwardPosition().z() 
            || z[1] < h.wire()->forwardPosition().z() ) ok[1] = false;
    }
    if ((! ok[0]) && (! ok[1])) {
        link.position(tmp);
        return -2;
    }*/
    //belle...
    if (link.leftRight() == 2) {
        if (z[0] < h.wire()->backwardPosition().z() - 20.
            || z[0] > h.wire()->forwardPosition().z() + 20.) ok[0] = false;
        if (z[1] < h.wire()->backwardPosition().z()-20.
            || z[1] > h.wire()->forwardPosition().z()+20.) ok[1] = false;
    }
    else {
        if (z[0] < h.wire()->backwardPosition().z()
            || z[0] > h.wire()->forwardPosition().z()) ok[0] = false;
        if (z[1] < h.wire()->backwardPosition().z()
            || z[1] > h.wire()->forwardPosition().z()) ok[1] = false;
    }
    if ((! ok[0]) && (! ok[1])) {
        link.position(tmp);
        return -2;
    }
    

    //...Cal. xy position of crossing points...
    HepVector3D p[2];
    p[0] = x + l[0] * v;
    p[1] = x + l[1] * v;
/*    if (_charge * (center.x() * p[0].y() - center.y() * p[0].x())  < 0.)  //liuqg, cosmic...
        ok[0] = false;
    if (_charge * (center.x() * p[1].y() - center.y() * p[1].x())  < 0.)
        ok[1] = false;
    if ((! ok[0]) && (! ok[1])){
      //        double tmp1 = _charge * (center.x() * p[0].y() - center.y() * p[0].x());
      //        double tmp2 = _charge * (center.x() * p[1].y() - center.y() * p[1].x()) ;
      //        if (link.leftRight() == 2) std::cout<<tmp1<<" "<<tmp2<<std::endl;
        link.position(tmp);
        return -3;
    }
*/
    //...Which one is the best?... Study needed...
    unsigned best = 0;
    if (ok[1]) best = 1;

    //...Cal. arc length...
    double cosdPhi = - center.dot((p[best] - center).unit()) / center.mag();
    double dPhi;
    if(fabs(cosdPhi)<=1.0) {
        dPhi = acos(cosdPhi);
    } else if (cosdPhi>1.0) {
        dPhi = 0.0;
    } else {
        dPhi = M_PI;
    }

    //...Finish...
    tmp.setX(r * dPhi);
    tmp.setY(z[best]);
    link.position(tmp);

    return 0;
}

int
TTrack::szPosition(const TSegment & segment, TMLink & a) const {

    //...Pick up a wire which represents segment position...
    AList<TMLink> links = segment.cores();
    unsigned n = links.length();
//    std::cout<<" szPosition TTrack:: segment.cores().length():"<<n<<endl;
//    for (unsigned i = 1; i < n; i++) {  
//       std::cout<<"drift distance"<<links[i]->hit()->drift()<<endl;
//    }  
    if (! n) return -1;
    TMLink * minL = links[0];
    float minDist = links[0]->drift();
#ifdef TRKRECO_DEBUG_DETAIL
    std::cout<<"minDist szPosition TTrack:"<<minDist<<endl;
#endif
    for (unsigned i = 1; i < n; i++) {
        if (links[i]->hit()->drift() < minDist) {
            minDist = links[i]->drift();
#ifdef TRKRECO_DEBUG_DETAIL
            std::cout<<"minDist szPosition TTrack:"<<minDist<<endl;   
#endif
            minL = links[i];
        }
    }

    //...sz calculation...
    a.position(minL->position());
    a.leftRight(2);
    a.hit(minL->hit());
    int err = szPosition(a);
#ifdef TRKRECO_DEBUG_DETAIL
        std::cout<<"err of szPosition TTrack:"<<err<<endl;      
#endif
    if (err) return -2;
    return 0;
}

int
TTrack::szPosition(const HepPoint3D & p, HepPoint3D & sz) const {

    //...Cal. arc length...
    HepPoint3D center = _helix->center();
    HepPoint3D xy = p;
    xy.setZ(0.);
    double cosdPhi = - center.dot((xy - center).unit()) / center.mag();
    double dPhi;
    if (fabs(cosdPhi) <= 1.0) {
        dPhi = acos(cosdPhi);
    }
    else if (cosdPhi > 1.0) {
        dPhi = 0.0;
    }
    else {
        dPhi = M_PI;
    }

    //...Finish...
    sz.setX(_helix->curv() * dPhi);
    sz.setY(p.z());
    sz.setZ(0.);

    return 0;
}

void
TTrack::assign(unsigned hitMask) {
    hitMask |= WireHitUsed;

    unsigned n = _links.length();
    for (unsigned i = 0; i < n; i++) {
        TMLink * l = _links[i];
        const TMDCWireHit * h = l->hit();
#ifdef TRKRECO_DEBUG
        if (h->track()) {
            std::cout << "TTrack::assign !!! hit(" << h->wire()->name();
            std::cout << ") already assigned" << std::endl;
        }
#endif

        //...This function will be moved to TTrackManager...
        h->track((TTrack *) this);
        h->state(h->state() | hitMask);
    }
}

Helix
Track2Helix(const MdcTrk_localz & t) {
    HepVector a(5);
    Hep3Vector p(t.pivot[0], t.pivot[1], t.pivot[2]);
    HepSymMatrix er(5,0);
    a(1) = t.helix[0];
    a(2) = t.helix[1];
    a(3) = t.helix[2];
    a(4) = t.helix[3];
    a(5) = t.helix[4];
    er(1,1) = t.error[0];
    er(2,1) = t.error[1];
    er(2,2) = t.error[2];
    er(3,1) = t.error[3];
    er(3,2) = t.error[4];
    er(3,3) = t.error[5];
    er(4,1) = t.error[6];
    er(4,2) = t.error[7];
    er(4,3) = t.error[8];
    er(4,4) = t.error[9];
    er(5,1) = t.error[10];
    er(5,2) = t.error[11];
    er(5,3) = t.error[12];
    er(5,4) = t.error[13];
    er(5,5) = t.error[14];
    return Helix(p, a, er);
}

Helix
Track2Helix(const MdcRec_trk & t) {
    HepVector a(5);
    Hep3Vector p(t.pivot[0], t.pivot[1], t.pivot[2]);
    HepSymMatrix er(5,0);
    a(1) = t.helix[0];
    a(2) = t.helix[1];
    a(3) = t.helix[2];
    a(4) = t.helix[3];
    a(5) = t.helix[4];
    er(1,1) = t.error[0];
    er(2,1) = t.error[1];
    er(2,2) = t.error[2];
    er(3,1) = t.error[3];
    er(3,2) = t.error[4];
    er(3,3) = t.error[5];
    er(4,1) = t.error[6];
    er(4,2) = t.error[7];
    er(4,3) = t.error[8];
    er(4,4) = t.error[9];
    er(5,1) = t.error[10];
    er(5,2) = t.error[11];
    er(5,3) = t.error[12];
    er(5,4) = t.error[13];
    er(5,5) = t.error[14];
    return Helix(p, a, er);
}

Helix
Track2Helix(const Mdst_trk_fit & t) {
    HepVector a(5);
    Hep3Vector p(t.pivot_x, t.pivot_y, t.pivot_z);
    HepSymMatrix er(5,0);
    a(1) = t.helix[0];
    a(2) = t.helix[1];
    a(3) = t.helix[2];
    a(4) = t.helix[3];
    a(5) = t.helix[4];
    er(1,1) = t.error[0];
    er(2,1) = t.error[1];
    er(2,2) = t.error[2];
    er(3,1) = t.error[3];
    er(3,2) = t.error[4];
    er(3,3) = t.error[5];
    er(4,1) = t.error[6];
    er(4,2) = t.error[7];
    er(4,3) = t.error[8];
    er(4,4) = t.error[9];
    er(5,1) = t.error[10];
    er(5,2) = t.error[11];
    er(5,3) = t.error[12];
    er(5,4) = t.error[13];
    er(5,5) = t.error[14];
    return Helix(p, a, er);
}

std::string
TrackKinematics(const Helix & h) {
    static const HepPoint3D IP(0., 0., 0.);
    Helix hIp = h;
    hIp.pivot(IP);

    float chrg = hIp.a()[2] / fabs(hIp.a()[2]);
    std::string s;
    if (chrg > 0.) s = "+";
    else           s = "-";

    float x[4];
    x[0] = fabs(hIp.a()[0]);
    x[1] = hIp.a()[3];
    x[2] = 1. / fabs(hIp.a()[2]);
    x[3] = (1. / fabs(hIp.a()[2])) * hIp.a()[4];

    if ((x[0] < 2.) && (fabs(x[1]) < 4.)) s += "i ";
    else                                  s += "  ";

    for (unsigned i = 0; i < 4; i++) {
        if (i) s += " ";

        if (x[i] < 0.) s += "-";
        else           s += " ";

        int y = int(fabs(x[i]));
        s += itostring(y) + ".";
        float z = fabs(x[i]);
        for (unsigned j = 0; j < 3; j++) {
            z *= 10.;
            y = (int(z) % 10);
            s += itostring(y);
        }
    }

    return s;
}

std::string
TrackStatus(const TTrack & t) {
    return TrackStatus(t.finder(),
                       t.type(),
                       t.quality(),
                       t.fitting(),
                       0,
                       0);
}

std::string
TrackStatus(const MdcRec_trk & c) {
//    const reccdc_trk_add & a =
//      * (reccdc_trk_add *) BsGetEnt(RECMDC_TRK_ADD, c.m_ID, BBS_No_Index);
//    return TrackStatus(a);
}

std::string
TrackStatus(const MdcRec_trk_add & a) {
//    return TrackStatus(a.decision,
//                     a.kind,
//                     a.quality,
//                     a.stat,
//                     a.mother,
//                     a.daughter);
}

std::string
TrackStatus(unsigned md,
            unsigned mk,
            unsigned mq,
            unsigned ms,
            unsigned mm,
            unsigned ma) {

    std::string f;
    if (md & TrackOldConformalFinder) f += "o";
    if (md & TrackFastFinder) f += "f";
    if (md & TrackSlowFinder) f += "s";
    if (md & TrackCurlFinder) f += "c";
    if (md & TrackTrackManager) f += "t";
    if (f == "") f = "?";

    std::string k;
    if (mk & TrackTypeNormal) k += "Norm";
    if (mk & TrackTypeCurl) k += "Curl";
    if (mk & TrackTypeCircle) k += "Circ";
    if (mk & TrackTypeIncomingCosmic) k += "Inco";
    if (mk & TrackTypeOutgoingCosmic) k += "Outc";
    if (mk & TrackTypeKink) k += "Kink";
    if (mk & TrackTypeSVDOnly) k += "Svd";
    if (k == "") k = "?";

    std::string b;
    if (mq & TrackQualityOutsideCurler) b += "Curlback";
    if (mq & TrackQualityAfterKink) b += "Afterkink";
    if (mq & TrackQualityCosmic) b += "Cosmic";
    if (mq & TrackQuality2D) b += "2D";
    if (b == "") b = "ok";

    std::string s;
    if (ms & TrackFitGlobal) s += "HFit";
    if (ms & TrackFitCosmic) s += "CFit";
    if (ms & TrackFitCdcKalman) s += "CKal";
    if (ms & TrackFitSvdCdcKalman) s += "SKal";
    if (s == "") s = "?";

    int m = mm;
    if (m) --m;

    int d = ma;
    if (d) --d;

    std::string p = " ";
    return f + p + k + p + b + p + s + p + itostring(m) + p + itostring(d);
}

std::string
TrackInformation(const TTrack & t) {
    const AList<TMLink> cores = t.cores();
    unsigned n = cores.length();
    unsigned nS = NStereoHits(cores);
    unsigned nA = n - nS;
    std::string p;
    if (! PositiveDefinite(t.helix())) p = " negative";
    if (HelixHasNan(t.helix())) p += " NaN";
    return TrackInformation(nA, nS, n, t.chi2()) + p;
}

std::string
TrackInformation(const MdcRec_trk & r) {
    std::string p;
    if (PositiveDefinite(Track2Helix(r))) p = " posi";
    else                                  p = " nega";
    if (HelixHasNan(Track2Helix(r))) p += " with NaN";
    return TrackInformation(r.nhits - r.nster,
                            r.nster,
                            r.nhits,
                            r.chiSq) + p;
}

std::string
TrackInformation(unsigned nA, unsigned nS, unsigned n, float chisq) {
    std::string s;

    s += "a" + itostring(int(nA));
    s += " s" + itostring(int(nS));
    s += " n" + itostring(int(n));
    // s += " ndf" + std::string(int(r.m_ndf));
    float x = chisq;

    if (x < 0.) s += " -";
    else        s += " ";

    int y = int(fabs(x));
    s += itostring(y) + ".";
    float z = fabs(x);
    for (unsigned j = 0; j < 1; j++) {
        z *= 10.;
        y = (int(z) % 10);
        s += itostring(y);
    }

    return s;
}

std::string
TrackLayerUsage(const TTrack & t) {
    unsigned n[11];
    NHitsSuperLayer(t.links(), n);
    std::string nh;
    for (unsigned i = 0; i < 11; i++) {
        nh += itostring(n[i]);
        if (i % 2) nh += "-";
        else if (i < 10) nh += ",";
    }
    return nh;
}

bool
PositiveDefinite(const Helix & h) {
    const SymMatrix & e = h.Ea();
    SymMatrix e2 = e.sub(1, 2);
    SymMatrix e3 = e.sub(1, 3);
    SymMatrix e4 = e.sub(1, 4);

    bool positive = true;
    if (e[0][0] <= 0.) positive = false;
    else if (e2.determinant() <= 0.) positive = false;
    else if (e3.determinant() <= 0.) positive = false;
    else if (e4.determinant() <= 0.) positive = false;
    else if (e.determinant() <= 0.) positive = false;
    return positive;
}

bool
HelixHasNan(const Helix & h) {
    const Vector & a = h.a();
    for (unsigned i = 0; i < 5; i++)
        if (isnan(a[i]))
            return true;
    const SymMatrix & Ea = h.Ea();
    for (unsigned i = 0; i < 5; i++)
        for (unsigned j = 0; j <= i; j++)
            if (isnan(Ea[i][j]))
                return true;
    return false;
}

Helix
Track2Helix(const Gen_hepevt & t) {
    float charge = 1;
    if (t.idhep == 11) charge = -1;
    else if (t.idhep == -11) charge = 1;
    else if (t.idhep == 13) charge = -1;
    else if (t.idhep == -13) charge = 1;
    else if (t.idhep == 211) charge = 1;
    else if (t.idhep == -211) charge = -1;
    else if (t.idhep == 321) charge = 1;
    else if (t.idhep == -321) charge = -1;
    else if (t.idhep == 2212) charge = 1;
    else if (t.idhep == -2212) charge = -1;
    else {
        std::cout << "Track2Helix(gen_hepevt) !!! charge of id=";
        std::cout << t.idhep << " is unknown" << std::endl;
    }

    Hep3Vector mom(t.P[0], t.P[1], t.P[2]);
    Hep3Vector pos(t.V[0] / 10., t.V[1] / 10., t.V[2] / 10.);
    return Helix(pos, mom, charge);
}

---