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

Go to the documentation of this file.

//-----------------------------------------------------------------------------
// $Id: TCosmicFitter.cxx,v 1.12 2011/12/05 04:49:50 maoh Exp $
//-----------------------------------------------------------------------------
// Filename : TCosmicFitter.cc
// Section  : Tracking
// Owner    : Yoshi Iwasaki
// Email    : yoshihito.iwasaki@kek.jp
//-----------------------------------------------------------------------------
// Description : A class to fit a TTrackBase object to a helix.
//               See http://bsunsrv1.kek.jp/~yiwasaki/tracking/
//-----------------------------------------------------------------------------

#define HEP_SHORT_NAMES
#include <float.h>
//#include "tables/cdc.h"
#include "CLHEP/Matrix/Vector.h"
#include "CLHEP/Matrix/SymMatrix.h"
#include "CLHEP/Matrix/DiagMatrix.h"
#include "CLHEP/Matrix/Matrix.h"
//#include "panther/panther.h"
//#include "panther/panther_manager.h"
#include "TrkReco/TCosmicFitter.h"
#include "TrkReco/TTrack.h"

#include "TrkReco/TrkReco.h"

#include "MdcCalibFunSvc/IMdcCalibFunSvc.h"
#include "MdcCalibFunSvc/MdcCalibFunSvc.h"

#include "GaudiKernel/MsgStream.h"
#include "GaudiKernel/AlgFactory.h"
#include "GaudiKernel/ISvcLocator.h"
#include "GaudiKernel/IMessageSvc.h"
#include "GaudiKernel/IDataProviderSvc.h"
#include "GaudiKernel/IDataManagerSvc.h"
#include "GaudiKernel/SmartDataPtr.h"
#include "GaudiKernel/IDataProviderSvc.h"
#include "GaudiKernel/PropertyMgr.h"
#include "GaudiKernel/IJobOptionsSvc.h"
#include "GaudiKernel/IMessageSvc.h"
#include "GaudiKernel/Bootstrap.h"
#include "GaudiKernel/StatusCode.h"

#include "GaudiKernel/ContainedObject.h"
#include "GaudiKernel/SmartRef.h"
#include "GaudiKernel/SmartRefVector.h"
#include "GaudiKernel/ObjectVector.h"
#include "EventModel/EventModel.h"

#include  "EvTimeEvent/RecEsTime.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;

typedef CLHEP::HepMatrix Matrix;

#define CAL_TOF_HELIX 0

#define NTrailMax 100
#define Convergence 1.0e-5

/*
extern "C" void calcdc_sag3_(int*, float*, float[3], float*, float*, float*);
extern "C" void calcdc_driftdist_(int*, int*, int*, 
                float[3], float[3], float*, float*, float*);
*/

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

}

TCosmicFitter::~TCosmicFitter() {
}

int
TCosmicFitter::fit(TTrackBase & b) const {

//  double t0_bes;
//  TrkReco::t0(t0_bes);
//  const double t0_bes = TrkReco::t0();

    //...Already fitted ?...
    if (b.fitted()) return TFitAlreadyFitted;

    int err = fit(b, 0.);
    if (! err) b._fitted = true;

    return err;
}

int
TCosmicFitter::fit(TTrackBase & b, float t0Offset) const {

#ifdef TRKRECO_DEBUG_DETAIL
    std::cout << "    TCosmicFitter::fit ..." << std::endl;
#endif

    IMdcCalibFunSvc* l_mdcCalFunSvc;
    Gaudi::svcLocator() -> service("MdcCalibFunSvc", l_mdcCalFunSvc);

    //...Check # of hits...
    if (b.links().length() < 5) return TFitErrorFewHits;
    unsigned nValid = 0;
    for (unsigned i = 0; i < b.links().length(); i++) {
        unsigned state = b.links()[i]->hit()->state();
        if (state & WireHitInvalidForFit) continue;
        if (state & WireHitFittingValid) ++nValid;
    }
    if (nValid < 5) return TFitErrorFewHits;

    //...Type check...
    //    if (b.type() != Track) return TFitUnavailable;
    if (b.objectType() != Track) return TFitUnavailable;
    TTrack & t = (TTrack &) b;

    //read t0 from TDS-------------------------------------//
    double _t0_bes = -1.;
    IMessageSvc *msgSvc;
    Gaudi::svcLocator()->service("MessageSvc", msgSvc);
    MsgStream log(msgSvc, "TCosmicFitter");

    IDataProviderSvc* eventSvc = NULL;
    Gaudi::svcLocator()->service("EventDataSvc", eventSvc);

    SmartDataPtr<RecEsTimeCol> aevtimeCol(eventSvc,"/Event/Recon/RecEsTimeCol");
    if (aevtimeCol) {
      RecEsTimeCol::iterator iter_evt = aevtimeCol->begin();
      _t0_bes = (*iter_evt)->getTest();
//      cout<<"  tev: "<<setw(4)<<_t0_bes<<endl;
    }else{
      log << MSG::WARNING << "Could not find RecEsTimeCol" << endreq;
    }
//    if (_t0_bes < 7.) _t0_bes = 7.;
    //----------------------------------------------------//

    //...Setup...
    Vector a(5), da(5);
    a = t.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;
    double chi2Old = DBL_MAX;
    const double convergence = Convergence;
    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...
    unsigned nTrial = 0;
    while (nTrial < NTrailMax) {

        //...Set up...
        chi2 = 0.;
        for (unsigned j = 0; j < 5; j++) dchi2da[j] = 0.;
        d2chi2d2a = SymMatrix(5, 0);
        
#if CAL_TOF_HELIX
        //cal the time pass through the chamber
        const float Rad = 81; // cm
        float dRho = t.helix().a()[0];
        float Lproj = sqrt(Rad*Rad - dRho*dRho);
        float tlmd = t.helix().a()[4];
        float fct = sqrt(1. + tlmd * tlmd);
#endif

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

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

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

            //...Cal. closest points...
            int doSagCorrection = 0;
//          if(nTrial && !allAxial ) doSagCorrection = 1; //Liuqg
            t.approach(* l, doSagCorrection);
            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.drift(leftRight);
            double eDistance = h.dDrift(leftRight);
            //... 
            if(nTrial  && !allAxial){
               int side = leftRight;

               double Tfly = _t0_bes/220.*(110.-onWire.y());
#if CAL_TOF_HELIX
               float Rad_wire = sqrt(x[0]*x[0]+x[1]*x[1]); // cm
               float L_wire = sqrt(Rad_wire*Rad_wire - dRho*dRho);
               float flyLength = Lproj - L_wire;
               if (x[1]<0) flyLength = Lproj + L_wire;
               Tfly = flyLength/29.98*sqrt(1.0+(0.105/0.5)*(0.105/0.5))*fct; //approxiate... cm/ns
#endif
               float time = l->hit()->reccdc()->tdc - Tfly;
          
               int wire    = h.wire()->localId();
               int layerId = h.wire()->layerId();
               float dist =  distance;
               float edist = eDistance;
           double T0 = l_mdcCalFunSvc->getT0(layerId,wire);
           double rawadc = 0.;
           rawadc =l->hit()->reccdc()->adc;

           double timeWalk = l_mdcCalFunSvc->getTimeWalk(layerId, rawadc);
           double drifttime =time -T0-timeWalk;
           dist = l_mdcCalFunSvc->driftTimeToDist(drifttime,layerId, wire, side, 0.0);
               edist = l_mdcCalFunSvc->getSigma(layerId, side, dist, 0.0);
               dist = dist/10.0; //mm->cm
               edist = edist/10.0;
//             if(layerId<20) edist = 9999.;

//zsl          calcdc_driftdist_(&doPropDelay,&wire,&side,p,x,&time,&dist,&edist);

               distance = (double) dist;
               eDistance = (double) edist;

               l->drift(distance,0);  //update
               l->drift(distance,1);
               l->dDrift(eDistance,0);
               l->dDrift(eDistance,1);
               l->tof(Tfly);
            }
            double eDistance2 = eDistance * eDistance;

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

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

            //...Chi2 related...
            // dDda = (v.x() * dxda + v.y() * dyda + v.z() * dzda) / vmag;
            HepVector3D 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;
//        a[2] = 0.000000001;
          t._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 = t.links()[i++]) {
              const TMDCWireHit & h = * l->hit();
            
            //...Check state...
            if (h.state() & WireHitInvalidForFit) continue;
            if (! (h.state() & WireHitFittingValid)) continue;

            //...Check wire...
            if (! nTrial)
              if (h.wire()->stereo()) allAxial = false;
            
            //...Cal. closest points...
            int doSagCorrection = 0;
//            if( nTrial  && !allAxial ) doSagCorrection = 1;  //Liuqg
            t.approach(* l, doSagCorrection);
            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.drift(leftRight);
            double eDistance = h.dDrift(leftRight);
            if(nTrial  && !allAxial){
               int side = leftRight;

               double Tfly = _t0_bes/220.*(110.-onWire.y());
#if CAL_TOF_HELIX
               float Rad_wire = sqrt(x[0]*x[0]+x[1]*x[1]); // cm
               float L_wire = sqrt(Rad_wire*Rad_wire - dRho*dRho);
               float flyLength = Lproj - L_wire;
               if (x[1]<0) flyLength = Lproj + L_wire;
               Tfly = flyLength/29.98*sqrt(1.0+(0.105/10)*(0.105/10))*fct; //approxiate... cm/ns
#endif

               float time = l->hit()->reccdc()->tdc - Tfly;

               int wire    = h.wire()->localId();
               int layerId = h.wire()->layerId();
               float dist= distance;
               float edist = eDistance;
//               int doPropDelay = 1; // 
           
           double T0 = l_mdcCalFunSvc->getT0(layerId,wire);
           double rawadc = 0.;
           rawadc =l->hit()->reccdc()->adc;
           double timeWalk = l_mdcCalFunSvc->getTimeWalk(layerId, rawadc);
           double drifttime =time -T0-timeWalk;
           dist = l_mdcCalFunSvc->driftTimeToDist(drifttime,layerId, wire, side, 0.0);
               edist = l_mdcCalFunSvc->getSigma(layerId, side, dist, 0.0);
               dist = dist/10.0; //mm->cm
               edist = edist/10.0;
//             if (layerId<20) edist = 9999.;

//zsl          calcdc_driftdist_(&doPropDelay,&wire,&side,p,x,&time,&dist,&edist);

               distance = (double) dist;
               eDistance = (double) edist;

               l->drift(distance,0);   //update
               l->drift(distance,1);
               l->dDrift(eDistance,0);
               l->dDrift(eDistance,1);
               l->tof(Tfly);
            }
            double eDistance2 = eDistance * eDistance;

            //...Residual...
            HepVector3D v = onTrack - onWire;
            double vmag = v.mag();
            double dDistance = vmag - distance;
            
            //...dxda...
            this->dxda(*l, t.helix(), dPhi, dxda, dyda, dzda, doSagCorrection);
            
            //...Chi2 related...
            //dDda = (v.x() * dxda + v.y() * dyda + v.z() * dzda) / vmag;
            HepVector3D 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;
//      a[2] = 0.000000001;
        t._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) {
        t._helix->a(a);
        t._helix->Ea(Ea);
    }
    else {
        err = TFitFailed;
    }

    t._ndf = nValid - dim;
    t._chi2 = chi2;
    // t._fitted = true;
    
    return err;
}

/*

// addition by matsu ( 1999/07/05 )
int 
TCosmicFitter::fitWithCathode( TTrackBase &b, float t0Offset,
                               float windowSize , int SysCorr ) {

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

    //...Already fitted ? ...
    if (b.fittedWithCathode()) return 0;

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

   //...Type check...
    if (b.objectType() != Track) return TFitUnavailable;
    TTrack & t = (TTrack &) b;

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

    //...Setup...
    unsigned nTrial = 0;
    Vector a(5), da(5);
    a = t.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 = DBL_MAX;
    const double convergence = Convergence;
    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 = t.catHits();

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

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

      //...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;
       AList<TMLink> cores = t.cores();
      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...
        int doSagCorrection = 0;
        if(nTrial && !allAxial ) doSagCorrection = 1; 
        t.approach(* l, doSagCorrection );
        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);
            //... 
            if(nTrial  && !allAxial){
               int wire = h.wire()->id();
               int side = leftRight;
               if(side==0) side = -1;
               float x[3]={ onWire.x(), onWire.y(), onWire.z()};
               float p[3]={t.p().x(), t.p().y(), t.p().z()};
               float time = l->hit()->reccdc()->tdc + t0Offset;
               float dist =  distance;
               float edist = eDistance;
               int doPropDelay = 1;
               calcdc_driftdist_(
               &doPropDelay,&wire,&side,p,x,&time,&dist,&edist);
               distance = (double) dist;
               eDistance = (double) edist;
            }
        double eDistance2 = eDistance * eDistance;

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

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

        // ... 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(t.helix().x(dPhi).z());

              if( SysCorr ){
                if( !nTrial ) {
                  mclust->zcalc( atan( t.helix().tanl()) );
                  mclust->esterz( atan( t.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)< windowSize && 
            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);
      

      } // TMLink 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;
          t._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 = 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;

     //...Cal. closest points...
       int doSagCorrection = 0;
       if( nTrial  && !allAxial ) doSagCorrection = 1; 
        t.approach(* l , doSagCorrection );
        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);
       if(nTrial  && !allAxial){
               int wire = h.wire()->id();
               int side = leftRight;
               if(side==0) side = -1;
               float x[3]={ onWire.x(), onWire.y(), onWire.z()};
               float p[3]={t.p().x(), t.p().y(), t.p().z()};
               float time = l->hit()->reccdc()->tdc + t0Offset;
               float dist= distance;
               float edist = eDistance;
               int doPropDelay = 1; // 
               calcdc_driftdist_(
               &doPropDelay,&wire,&side,p,x,&time,&dist,&edist);
               distance = (double) dist;
               eDistance = (double) edist;
         }
        double eDistance2 = eDistance * eDistance;

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

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

        // ... 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(t.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;
      t._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) {
      t._helix->a(a);
      t._helix->Ea(Ea);
    } else {
      err = TFitFailed;
    }
      
    t._ndf = t.nCores() + NusedCathode - dim;

    t._chi2 = chi2;

    t._fittedWithCathode = true;
    
    return err;
}

// end of addition

*/


int
TCosmicFitter::dxda(const TMLink & link,
                    const Helix & h,
                    double dPhi,
                    Vector & dxda,
                    Vector & dyda,
                    Vector & dzda,
                    int doSagCorrection) const {

    //...Setup...
    const TMDCWire & w = * link.wire();
    Vector a = h.a();
    double dRho  = a[0];
    double phi0  = a[1];
    double kappa = a[2];
   // double rho   = Helix::ConstantAlpha / kappa;
   // double rho   = 333.564095 / kappa;
    double rho   = 333.564095/(-1000*(m_pmgnIMF->getReferField())) / kappa;    
cout<<"TCosmicFitter::rho: "<<333.564095/(-1000*(m_pmgnIMF->getReferField()))<<endl;
    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);
    //... sag correction
    HepPoint3D xw = w.xyPosition();
    HepPoint3D wireBackwardPosition = w.backwardPosition();
    HepVector3D v = w.direction();

    if(doSagCorrection ){
      cout<<"doSagCorrection in TCosmicFitter!"<<endl;
      int  wireID = w.id();
      float wirePosition[3];
      float wireZ = link.positionOnTrack().z();
      float dydz =0;
      float ybSag = 0;
      float yfSag = 0;
      if(wireZ >w.backwardPosition().z() &&
         wireZ < w.forwardPosition().z()    ){

//zsl        calcdc_sag3_(&wireID, &wireZ, wirePosition, &dydz, &ybSag, &yfSag);   
  
        //... wire Position
        xw.setX( (double) wirePosition[0]);
        xw.setY( (double) wirePosition[1]);
        xw.setZ( (double) wirePosition[2]);
        //...
        wireBackwardPosition.setY((double) ybSag);
        //...
        //...        v.setY((double) dydz);
        HepVector3D v_aux(w.forwardPosition().x() - w.backwardPosition().x(),
                           yfSag - ybSag,
                           w.forwardPosition().z() - w.backwardPosition().z());
        v = v_aux.unit();      
      }
    }

    //...Axial case...
    if (w.axial()) {
      //        HepPoint3D d = h.center() - w.xyPosition();
        HepPoint3D d = h.center() - xw;
        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 = h.x(dPhi);
        Vector c(3);
        //c = HepPoint3D(w.backwardPosition() - (v * w.backwardPosition()) * v);
        c = HepPoint3D(wireBackwardPosition - (v * wireBackwardPosition) * 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;
}

---