/home/bes3soft/bes3soft/Boss/7.0.2/dist/7.0.2/Reconstruction/KalFitAlg/KalFitAlg-00-07-55-p03/src/KalFitTrack.cxx

Go to the documentation of this file.

//#include "TrackUtil/Helix.h"
#include "KalFitAlg/helix/Helix.h"
#include "KalFitAlg/KalFitWire.h"
#include "KalFitAlg/KalFitTrack.h"
#include "KalFitAlg/KalFitMaterial.h"
#include "KalFitAlg/KalFitElement.h"
#include "KalFitAlg/KalFitCylinder.h"
#include "MdcGeomSvc/MdcGeomSvc.h"
#include "GaudiKernel/Bootstrap.h"
#include "CLHEP/Matrix/Vector.h"
#include "CLHEP/Matrix/Matrix.h"
#include "CLHEP/Matrix/SymMatrix.h"
#include "CLHEP/Vector/ThreeVector.h"
#include "CLHEP/Units/PhysicalConstants.h"
#include "Math/Point3Dfwd.h"
#include "Math/Point3D.h"
#include "Math/Vector3D.h"

using namespace ROOT::Math;

using namespace KalmanFit;

using namespace CLHEP;


// for debug purpose .
int KalFitTrack::drifttime_choice_ = 0;
int KalFitTrack::debug_ = 0;
int KalFitTrack::numf_ = 0;
int KalFitTrack::numfcor_ = 1;
double KalFitTrack::Bznom_ = 10;
int KalFitTrack::steplev_ = 0;
int KalFitTrack::inner_steps_ = 3;
int KalFitTrack::outer_steps_ = 3;
// Mdc factors :
int KalFitTrack::Tof_correc_ = 1;
int KalFitTrack::strag_ = 1;
double KalFitTrack::chi2_hitf_ = 1000;
double KalFitTrack::chi2_hits_ = 1000;
double KalFitTrack::factor_strag_ = 0.4;
//double KalFitTrack::chi2_hitf_ = DBL_MAX;
int KalFitTrack::lead_ = 2;
int KalFitTrack::back_ = 1;
// attention resolflag !!!
int KalFitTrack::resolflag_ = 0;
int KalFitTrack::tofall_ = 1;
// chi2 cut set :
int KalFitTrack::nmdc_hit2_ = 500;
//int KalFitTrack::LR_ = 0;
int KalFitTrack::LR_ = 1;


double KalFitTrack::dchi2cutf_anal[43] = {0.};
double KalFitTrack::dchi2cuts_anal[43] = {0.};
double KalFitTrack::dchi2cutf_calib[43] = {0.};
double KalFitTrack::dchi2cuts_calib[43] = {0.};


double KalFitTrack::mdcGasRadlen_ = 0.;


const double KalFitTrack::MASS[NMASS] = { 0.000510999,
        0.105658,
        0.139568,
        0.493677,
        0.938272 };
const double
M_PI2 = 2. * M_PI;

const double
M_PI4 = 4. * M_PI;

const double
M_PI8 = 8. * M_PI;



// constructor
KalFitTrack::KalFitTrack(const HepPoint3D& pivot,
                const HepVector& a,
                const HepSymMatrix& Ea,
                unsigned int m, double chisq, unsigned int nhits)
: Helix(pivot, a, Ea), type_(0), insist_(0), chiSq_(0),
        nchits_(0), nster_(0), ncath_(0),
        ndf_back_(0), chiSq_back_(0), pathip_(0), 
        path_rd_(0), path_ab_(0), tof_(0), dchi2_max_(0), r_max_(0),
        tof_kaon_(0), tof_proton_(0), pat1_(0), pat2_(0), layer_prec_(0),
        trasan_id_(0), nhit_r_(0), nhit_z_(0),pathSM_(0),tof2_(0)
{
        memset(PathL_, 0, sizeof(PathL_));
        l_mass_ = m;
        if (m < NMASS) mass_ = MASS[m];
        else mass_ = MASS[2];
        r0_ = fabs(center().perp() - fabs(radius()));
        //bFieldZ(Bznom_);
        Bznom_=bFieldZ(); // 2012-09-13 wangll
        update_last();
}

// destructor
KalFitTrack::~KalFitTrack(void)
{
        // delete all objects

}

void KalFitTrack::update_last(void)
{
        pivot_last_ = pivot();
        a_last_ = a();
        Ea_last_ = Ea();
}

void KalFitTrack::update_forMdc(void)
{
        pivot_forMdc_ = pivot();
        a_forMdc_ = a();
        Ea_forMdc_ = Ea();
}

double KalFitTrack::intersect_cylinder(double r) const
{
        double m_rad = radius();
        double l = center().perp();

        double cosPhi = (m_rad * m_rad + l * l  - r * r) / (2 * m_rad * l);

        if(cosPhi < -1 || cosPhi > 1) return 0;

        double dPhi = center().phi() - acos(cosPhi) - phi0();

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

        return dPhi;
}

double KalFitTrack::intersect_zx_plane(const HepTransform3D& plane,
                double y) const
{
        HepPoint3D xc = plane * center();
        double r = radius();
        double d = r * r - (y - xc.y()) * (y - xc.y());
        if(d < 0) return 0;

        double xx = xc.x();
        if(xx > 0) xx -= sqrt(d);
        else xx += sqrt(d);

        double l = (plane.inverse() *
                        HepPoint3D(xx, y, 0)).perp();

        return intersect_cylinder(l);
}

double KalFitTrack::intersect_yz_plane(const HepTransform3D& plane,
                double x) const
{
        HepPoint3D xc = plane * center();
        double r = radius();
        double d = r * r - (x - xc.x()) * (x - xc.x());
        if(d < 0) return 0;

        double yy = xc.y();
        if(yy > 0) yy -= sqrt(d);
        else yy += sqrt(d);

        double l = (plane.inverse() *
                        HepPoint3D(x, yy, 0)).perp();

        return intersect_cylinder(l);
}

double KalFitTrack::intersect_xy_plane(double z) const
{
        if (tanl() != 0 && radius() != 0)
                return (pivot().z() + dz() - z) / (radius() * tanl());
        else return 0;
}

void KalFitTrack::ms(double path,
                const KalFitMaterial& material, int index)
{
        HepSymMatrix ea = Ea();
        //cout<<"ms():path  "<<path<<endl;
        //cout<<"ms():ea before: "<<ea<<endl;
        double k = kappa();
        double t = tanl();
        double t2 = t * t;
        double pt2 = 1 + t2;
        double k2 = k * k;

        double pmag = 1 / fabs(k) * sqrt(pt2);
        double dth = material.mcs_angle(mass_, path, pmag);
        double dth2 = dth * dth;
        double pt2dth2 = pt2 * dth2;

        ea[1][1] += pt2dth2;
        ea[2][2] += k2 * t2 * dth2;
        ea[2][4] += k * t * pt2dth2;
        ea[4][4] += pt2 * pt2dth2;

        ea[3][3] += dth2 * path * path /3 / (1 + t2);
        ea[3][4] += dth2 * path/2 * sqrt(1 + t2);
        ea[3][2] += dth2 * t / sqrt(1 + t2) * k * path/2;
        ea[0][0] += dth2 * path * path/3;
        ea[0][1] += dth2 * sqrt(1 + t2) * path/2;

        Ea(ea);
        //cout<<"ms():ms angle in this: "<<dth<<endl;  
        //cout<<"ms():ea after: "<<Ea()<<endl;
        if (index < 0) {
                double x0 = material.X0();
                if (x0) path_rd_ += path/x0;
        }
}

void KalFitTrack::msgasmdc(double path, int index)
{
        double k = kappa();
        double t = tanl();
        double t2 = t * t;
        double k2 = k * k;

        double pmag = ( 1 / fabs(k) ) * sqrt(1 + t2);
        double psq = pmag*pmag;
        /*
           double Zprims = 3/2*0.076 + 0.580/9.79*4.99*(4.99+1) +
           0.041/183.85*74*(74+1) + 0.302/26.98 * 13 * (13+1);
           double chicc = 0.00039612 * sqrt(Zprims * 0.001168);
           double dth = 2.557 * chicc * sqrt(path * (mass_*mass_ + psq)) / psq;
         */

        //std::cout<<" mdcGasRadlen: "<<mdcGasRadlen_<<std::endl;
        double pathx = path/mdcGasRadlen_;
        double dth =  0.0136* sqrt(pathx * (mass_*mass_ + psq))/psq 
                *(1 + 0.038 * log(pathx));;
        HepSymMatrix ea = Ea();
#ifdef YDEBUG
        cout<<"msgasmdc():path  "<<path<<"  pathx "<<pathx<<endl;
        cout<<"msgasmdc():dth  "<<dth<<endl;
        cout<<"msgasmdc():ea before: "<<ea<<endl;
#endif
        double dth2 = dth * dth;

        ea[1][1] += (1 + t2) * dth2;
        ea[2][2] += k2 * t2 * dth2;
        ea[2][4] += k * t * (1 + t2) * dth2;
        ea[4][4] += (1 + t2) * (1 + t2) * dth2;

        // additionnal terms (terms proportional to l and l^2)

        ea[3][3] += dth2 * path * path /3 / (1 + t2);
        ea[3][4] += dth2 * path/2 * sqrt(1 + t2);
        ea[3][2] += dth2 * t / sqrt(1 + t2) * k * path/2;
        ea[0][0] += dth2 * path * path/3;
        ea[0][1] += dth2 * sqrt(1 + t2) * path/2;

        Ea(ea);
#ifdef YDEBUG
        cout<<"msgasmdc():ea after: "<<Ea()<<endl;
#endif
        if (index < 0) {
                pathip_ += path;
                // RMK : put by hand, take care !!
                double x0 = mdcGasRadlen_; // for the Mdc gas
                path_rd_ += path/x0;
                tof(path);
#ifdef YDEBUG
                cout<<"ms...pathip..."<<pathip_<<endl;
#endif
        }
}

void KalFitTrack::eloss(double path,
                const KalFitMaterial& material, int index)
{
#ifdef YDEBUG
        cout<<"eloss():ea before: "<<Ea()<<endl;
#endif
        HepVector v_a = a();
        double v_a_2_2 = v_a[2] * v_a[2];
        double v_a_4_2 = v_a[4] * v_a[4];
        double pmag = 1 / fabs(v_a[2]) * sqrt(1 + v_a_4_2);
        double psq = pmag * pmag;
        double E = sqrt(mass_ * mass_ + psq);
        double dE = material.dE(mass_, path, pmag);
        //std::cout<<" eloss(): dE: "<<dE<<std::endl;//wangll

        if (index > 0) 
                psq += dE * (dE + 2 * sqrt(mass_ * mass_ + psq));
        else {
                double dE_max = E - mass_;
                if( dE<dE_max ) psq += dE * (dE - 2 * sqrt(mass_ * mass_ + psq));
                else psq=-1.0;
        }

        if (tofall_ && index < 0){
                // Kaon case :
                if (p_kaon_ > 0){
                        double psq_kaon = p_kaon_ * p_kaon_;
                        double dE_kaon = material.dE(MASS[3], path, p_kaon_);
                        psq_kaon += dE_kaon * (dE_kaon - 
                                        2 * sqrt(MASS[3] * MASS[3] + psq_kaon));
                        if (psq_kaon < 0) psq_kaon = 0;
                        p_kaon_ = sqrt(psq_kaon);
                }
                // Proton case :
                if (p_proton_ > 0){
                        double psq_proton = p_proton_ * p_proton_;
                        double dE_proton = material.dE(MASS[4], path, p_proton_);
                        psq_proton += dE_proton * (dE_proton - 
                                        2 * sqrt(MASS[4] * MASS[4] + psq_proton));
                        if (psq_proton < 0) psq_proton = 0;
                        p_proton_ = sqrt(psq_proton);
                }
        }

        double dpt;
        //cout<<"eloss(): psq = "<<psq<<endl;//wangll
        if(psq < 0) dpt = 9999;
        else dpt = v_a[2] * pmag / sqrt(psq);
        //cout<<"eloss():k:   "<<v_a[2]<<"  k'  "<<dpt<<endl;//wangll
#ifdef YDEBUG
        cout<<"eloss():k:   "<<v_a[2]<<"  k'  "<<dpt<<endl;
#endif
        // attempt to take account of energy loss for error matrix 

        HepSymMatrix ea = Ea();
        double r_E_prim = (E + dE)/E;

        // 1/ Straggling in the energy loss :
        if (strag_){
                double del_E(0);
                if(l_mass_==0) {
                        del_E = dE*factor_strag_;
                }   else  {
                        del_E  = material.del_E(mass_, path, pmag);
                }

                ea[2][2] += (v_a[2] * v_a[2]) * E * E * del_E * del_E / (psq*psq);
        }

        // Effect of the change of curvature (just variables change):
        double dpt2 = dpt*dpt;
        double A = dpt*dpt2/(v_a_2_2*v_a[2])*r_E_prim;
        double B = v_a[4]/(1+v_a_4_2)*
                dpt*(1-dpt2/v_a_2_2*r_E_prim);

        double ea_2_0 = A*ea[2][0] + B*ea[4][0];
        double ea_2_1 = A*ea[2][1] + B*ea[4][1];
        double ea_2_2 = A*A*ea[2][2] + 2*A*B*ea[2][4] + B*B*ea[4][4]; 
        double ea_2_3 = A*ea[2][3] + B*ea[4][3]; 
        double ea_2_4 = A*ea[2][4] + B*ea[4][4];

        v_a[2] = dpt;
        a(v_a);

        ea[2][0] = ea_2_0;
        //std::cout<<"ea[2][0] is "<<ea[2][0]<<" ea(3,1) is "<<ea(3,1)<<std::endl;
        ea[2][1] = ea_2_1;
        //std::cout<<"ea[2][2] is "<<ea[2][2]<<" ea(3,3) is "<<ea(3,3)<<std::endl;
        ea[2][2] = ea_2_2;
        ea[2][3] = ea_2_3;
        ea[2][4] = ea_2_4;

        Ea(ea);
        //cout<<"eloss():dE:   "<<dE<<endl;
        //cout<<"eloss():A:   "<<A<<"   B:    "<<B<<endl;
        //cout<<"eloss():ea after: "<<Ea()<<endl;
        r0_ = fabs(center().perp() - fabs(radius()));
}

void KalFitTrack::order_wirhit(int index)
{
        unsigned int nhit = HitsMdc_.size();
        Helix tracktest = *(Helix*)this;
        int ind = 0;
        double* Rad = new double[nhit];
        double* Ypos = new double[nhit];
        for( unsigned i=0 ; i < nhit; i++ ){

                HepPoint3D fwd(HitsMdc_[i].wire().fwd());
                HepPoint3D bck(HitsMdc_[i].wire().bck());
                Hep3Vector wire = (CLHEP::Hep3Vector)fwd -(CLHEP::Hep3Vector)bck;

                // Modification for stereo wires :
                Helix work = tracktest;
                work.ignoreErrorMatrix();
                work.pivot((fwd + bck) * .5);
                HepPoint3D x0 = (work.x(0).z() - bck.z())
                        / wire.z() * wire + bck;

                tracktest.pivot(x0);
                Rad[ind] = tracktest.x(0).perp();
                Ypos[ind] = x0.y();
                ind++;
                //cout<<"Ypos: "<<Ypos[ind-1]<<endl;
        }

        // Reorder...
        if (index < 0)
                for(int j, k = nhit - 1; k >= 0; k = j){
                        j = -1;
                        for(int i = 1; i <= k; i++)
                                if(Rad[i - 1] > Rad[i]){
                                        j = i - 1;
                                        std::swap(Rad[i], Rad[j]);
                                        std::swap(HitsMdc_[i], HitsMdc_[j]);
                                }
                }
        if (index > 0)
                for(int j, k = nhit - 1; k >= 0; k = j){
                        j = -1;
                        for(int i = 1; i <= k; i++)
                                if(Rad[i - 1] < Rad[i]){
                                        j = i - 1;
                                        std::swap(Rad[i], Rad[j]);
                                        std::swap(HitsMdc_[i], HitsMdc_[j]);
                                }
                }
        if (index == 0)
                for(int j, k = nhit - 1; k >= 0; k = j){
                        j = -1;
                        for(int i = 1; i <= k; i++)
                                if(Ypos[i - 1] > Ypos[i]){
                                        j = i - 1;
                                        std::swap(Ypos[i], Ypos[j]);
                                        std::swap(HitsMdc_[i], HitsMdc_[j]);
                                }
                }
        delete [] Rad;
        delete [] Ypos;
}

void KalFitTrack::order_hits(void){
        for(int it=0; it<HitsMdc().size()-1; it++){
                if(HitsMdc_[it].wire().layer().layerId() == HitsMdc_[it+1].wire().layer().layerId())    
                {
                        if((kappa()<0)&&(HitsMdc_[it].wire().localId() > HitsMdc_[it+1].wire().localId())){
                                std::swap(HitsMdc_[it], HitsMdc_[it+1]);
                        }
                        if((kappa()>0)&&(HitsMdc_[it].wire().localId() < HitsMdc_[it+1].wire().localId())){
                                std::swap(HitsMdc_[it], HitsMdc_[it+1]);
                        }
                }
        }
}


void KalFitTrack::number_wirhit(void)
{
        unsigned int nhit = HitsMdc_.size();
        int Num[50] = {0};
        for( unsigned i=0 ; i < nhit; i++ )
                Num[HitsMdc_[i].wire().layer().layerId()]++;
        for( unsigned i=0 ; i < nhit; i++ )
                if (Num[HitsMdc_[i].wire().layer().layerId()]>2)
                        HitsMdc_[i].chi2(-2);
}


double KalFitTrack::smoother_Mdc(KalFitHitMdc& HitMdc, Hep3Vector& meas, KalFitHelixSeg& seg, double& dchi2, int csmflag)
{
        // 
        double lr = HitMdc.LR();
        // For taking the propagation delay into account :
        int wire_ID = HitMdc.wire().geoID(); // from 0 to 6796
        int layerid = HitMdc.wire().layer().layerId();
        double entrangle = HitMdc.rechitptr()->getEntra();
        if (wire_ID<0 || wire_ID>6796){ //bes
                std::cout << "KalFitTrack : wire_ID problem : " << wire_ID << std::endl;
                return 0;
        } 

        double x[3] ={pivot().x(), pivot().y(), pivot().z()};
        double pmom[3] ={momentum().x(), momentum().y(), momentum().z()};

        double tofest(0);
        double dd(0.);
        double phi = fmod(phi0() + M_PI4, M_PI2);
        double csf0 = cos(phi);
        double snf0 = (1. - csf0) * (1. + csf0);
        snf0 = sqrt((snf0 > 0.) ? snf0 : 0.);
        if(phi > M_PI) snf0 = - snf0;

        if (Tof_correc_) {
                Hep3Vector ip(0, 0, 0);
                Helix work = *(Helix*)this;
                work.ignoreErrorMatrix();
                work.pivot(ip);
                double phi_ip = work.phi0();
                if (fabs(phi - phi_ip) > M_PI) {
                        if (phi > phi_ip) phi -= 2 * M_PI;
                        else phi_ip -= 2 * M_PI;
                }
                double t = tanl();
                double l = fabs(radius() * (phi - phi_ip) * sqrt(1 + t * t));
                double pmag( sqrt( 1.0 + t*t ) / kappa());
                double mass_over_p( mass_ / pmag );
                double beta( 1.0 / sqrt( 1.0 + mass_over_p * mass_over_p ) );
                tofest = l / ( 29.9792458 * beta );
                if(csmflag==1 && HitMdc.wire().y()>0.)  tofest= -1. * tofest;
        }

        const HepSymMatrix& ea = Ea();
        const HepVector& v_a = a();

        HepPoint3D pivot_work = pivot();

        double dchi2R(99999999), dchi2L(99999999);

        HepVector v_H(5, 0);
        v_H[0] =  -csf0 * meas.x() - snf0 * meas.y();
        v_H[3] =  -meas.z();
        HepMatrix v_HT = v_H.T();

        double estim = (v_HT * v_a)[0];
        HepVector ea_v_H = ea * v_H;  
        HepMatrix ea_v_HT = (ea_v_H).T();
        HepVector v_H_T_ea_v_H = v_HT * ea_v_H;

        HepSymMatrix eaNew(5);
        HepVector aNew(5);

        //double time = HitMdc.tdc();
        // if (Tof_correc_)
        //  time -= tofest;

        double drifttime = getDriftTime(HitMdc , tofest); 
        //cout<<"drifttime = "<<drifttime<<endl;
        seg.dt(drifttime);
        double ddl = getDriftDist(HitMdc, drifttime, 0);
        double ddr = getDriftDist(HitMdc, drifttime, 1);
        /*cout<<endl<<" $$$  smoother_Mdc():: "<<endl;//wangll
        cout<<"pivot = "<<pivot()<<endl;//wangll
        cout<<"helix = "<<a()<<endl;//wangll
        cout<<"drifttime = "<<drifttime<<endl;//wangll
        cout<<"ddl, ddr = "<<ddl<<", "<<ddr<<endl;//wangll
        */
        double erddl = getSigma( HitMdc, a()[4], 0, ddl);
        double erddr = getSigma( HitMdc, a()[4], 1, ddr); 

        //  double dd = 1.0e-4 * 40.0 * time;
        double dmeas2[2] = { 0.1*ddl, 0.1*ddr }; //millimeter to centimeter 
        double er_dmeas2[2] = {0. , 0.};
        if(resolflag_== 1) { 
                er_dmeas2[0] = 0.1*erddl;
                er_dmeas2[1] = 0.1*erddr;
        } 
        else if(resolflag_ == 0) {
                //int layid = HitMdc.wire().layer().layerId();
                //double sigma = getSigma(layid, dd);
                //er_dmeas2[0] = er_dmeas2[1] = sigma;
        }

        // Left hypothesis :
        if (lr == -1) {
                double er_dmeasL, dmeasL;
                if(Tof_correc_) {
                        dmeasL = (-1.0)*dmeas2[0];  // the dmeas&erdmeas  calculated by myself 
                        er_dmeasL = er_dmeas2[0];
                } else {
                        dmeasL = (-1.0)*fabs(HitMdc.dist()[0]); // the dmeas&erdmeas  calculated by track-finding algorithm
                        er_dmeasL = HitMdc.erdist()[0];
                }

                double AL = 1 / ((v_H_T_ea_v_H)[0] + er_dmeasL*er_dmeasL);
                eaNew.assign(ea - ea_v_H * AL * ea_v_HT);
                double RkL = 1 - (v_H_T_ea_v_H)[0] * AL;
                if(0. == RkL) RkL = 1.e-4;

                HepVector diffL = ea_v_H * AL * (dmeasL - estim);
                aNew = v_a + diffL;
                double sigL = (dmeasL - (v_HT * aNew)[0]);
                dchi2L =  (sigL*sigL) /  (RkL * er_dmeasL*er_dmeasL);
        } else if (lr == 1) {
                // Right hypothesis :

                double er_dmeasR, dmeasR;
                if(Tof_correc_) {
                        dmeasR = dmeas2[1];
                        er_dmeasR = er_dmeas2[1];
                } else {
                        dmeasR = fabs(HitMdc.dist()[1]);
                        er_dmeasR = HitMdc.erdist()[1];
                }


                double AR = 1 / ((v_H_T_ea_v_H)[0] + er_dmeasR*er_dmeasR);
                eaNew.assign(ea - ea_v_H * AR * ea_v_HT);
                double RkR = 1 - (v_H_T_ea_v_H)[0] * AR;
                if(0. == RkR) RkR = 1.e-4;

                HepVector diffR = ea_v_H * AR * (dmeasR - estim);
                aNew = v_a + diffR;
                double sigR = (dmeasR- (v_HT * aNew)[0]);
                dchi2R =  (sigR*sigR) /  (RkR * er_dmeasR*er_dmeasR);
        }

        // Update Kalman result :
#ifdef YDEBUG
        cout<<"in smoother_Mdc: lr= "<<lr<<" a: "<<v_a<<" a_NEW: "<<aNew<<endl;
#endif
        double dchi2_loc(0);
        if ((dchi2R < dchi2cuts_anal[layerid] && lr == 1) || 
                        (dchi2L < dchi2cuts_anal[layerid] && lr == -1)) {
                ndf_back_++;
                int chge(1);
                if ( !( aNew[2] && fabs(aNew[2]-a()[2]) < 1.0 ) ) chge=0;
                if (lr == 1) {
                        chiSq_back_ += dchi2R;
                        dchi2_loc = dchi2R;
                        dd = 0.1*ddr;
                } else if (lr == -1) {
                        chiSq_back_ += dchi2L;
                        dchi2_loc = dchi2L;
                        dd = -0.1*ddl;
                } 
                if (chge){
                        a(aNew);
                        Ea(eaNew);
                }
        }
        dchi2=dchi2_loc;

        seg.doca_include(fabs((v_HT*a())[0]));
        seg.doca_exclude(fabs(estim));
        Hep3Vector ip(0, 0, 0);
        KalFitTrack helixNew1(pivot_work, a(), Ea(), 0, 0, 0);
        helixNew1.pivot(ip);
        HepVector    a_temp1  = helixNew1.a();
        HepSymMatrix ea_temp1 = helixNew1.Ea();
        seg.Ea(ea_temp1);
        seg.a(a_temp1);
        seg.Ea_include(ea_temp1);
        seg.a_include(a_temp1);

        KalFitTrack helixNew2(pivot_work, v_a, ea, 0, 0, 0);
        helixNew2.pivot(ip);
        HepVector    a_temp2  = helixNew2.a();
        HepSymMatrix ea_temp2 = helixNew2.Ea();
        seg.Ea_exclude(ea_temp2);
        seg.a_exclude(a_temp2);

        seg.tof(tofest);
        seg.dd(dd);

        return chiSq_back_;
}

// Smoothing procedure in Mdc cosmic align
// RMK attention for the case chi2R = chi2L during forward filter !
double KalFitTrack::smoother_Mdc_csmalign(KalFitHelixSeg& seg, Hep3Vector& meas,int& flg, int csmflag )
{


        HepPoint3D ip(0., 0., 0.);
        // attention! we should not to decide the left&right in the smoother process,
        // because we choose the left&right of hits from the filter process. 

        KalFitHitMdc* HitMdc = seg.HitMdc();
        double lr = HitMdc->LR();

        // For taking the propagation delay into account :
        int layerid  = (*HitMdc).wire().layer().layerId();
        int wire_ID = HitMdc->wire().geoID();
        double entrangle = HitMdc->rechitptr()->getEntra();

        if (wire_ID<0 || wire_ID>6796){ //bes
                std::cout << "KalFitTrack : wire_ID problem : " << wire_ID << std::endl;
                return 0;
        } 

        double x[3] ={pivot().x(), pivot().y(), pivot().z()};
        double pmom[3] ={momentum().x(), momentum().y(), momentum().z()};
        double dd(0.);
        double tofest(0);
        double phi = fmod(phi0() + M_PI4, M_PI2);
        double csf0 = cos(phi);
        double snf0 = (1. - csf0) * (1. + csf0);
        snf0 = sqrt((snf0 > 0.) ? snf0 : 0.);
        if(phi > M_PI) snf0 = - snf0;

        if (Tof_correc_) {
                Hep3Vector ip(0, 0, 0);
                Helix work = *(Helix*)this;
                work.ignoreErrorMatrix();
                work.pivot(ip);
                double phi_ip = work.phi0();
                if (fabs(phi - phi_ip) > M_PI) {
                        if (phi > phi_ip) phi -= 2 * M_PI;
                        else phi_ip -= 2 * M_PI;
                }
                double t = tanl();
                double l = fabs(radius() * (phi - phi_ip) * sqrt(1 + t * t));
                double pmag( sqrt( 1.0 + t*t ) / kappa());
                double mass_over_p( mass_ / pmag );
                double beta( 1.0 / sqrt( 1.0 + mass_over_p * mass_over_p ) );
                tofest = l / ( 29.9792458 * beta );
                if(csmflag==1 && (*HitMdc).wire().y()>0.)  tofest= -1. * tofest;
        }

        const HepSymMatrix& ea = Ea();
        const HepVector& v_a = a();


        HepPoint3D pivot_work = pivot();

        /*
           HepVector a_work = a();
           HepSymMatrix ea_work = Ea();

           KalFitTrack helix_work(pivot_work, a_work, ea_work, 0, 0, 0);
           helix_work.pivot(ip);

           HepVector a_temp = helix_work.a();
           HepSymMatrix ea_temp = helix_work.Ea();

           seg.Ea_pre_bwd(ea_temp);     
           seg.a_pre_bwd(a_temp);

         */

        seg.a_pre_bwd(a()); 
        seg.Ea_pre_bwd(Ea()); 

        double dchi2R(99999999), dchi2L(99999999);
        HepVector v_H(5, 0);
        v_H[0] =  -csf0 * meas.x() - snf0 * meas.y();
        v_H[3] =  -meas.z();
        HepMatrix v_HT = v_H.T();

        double estim = (v_HT * v_a)[0];
        HepVector ea_v_H = ea * v_H;  
        HepMatrix ea_v_HT = (ea_v_H).T();
        HepVector v_H_T_ea_v_H = v_HT * ea_v_H;
        HepSymMatrix eaNew(5);
        HepVector aNew(5);
        double time = (*HitMdc).tdc();

        //seg.dt(time);
        // if (Tof_correc_)
        // { 
        //  time -= tofest;
        //  seg.dt(time);
        // }
        // double dd = 1.0e-4 * 40.0 * time;
        // seg.dd(dd);

        double drifttime = getDriftTime(*HitMdc , tofest);
        seg.dt(drifttime);
        double ddl = getDriftDist(*HitMdc, drifttime, 0 );
        double ddr = getDriftDist(*HitMdc, drifttime, 1 );
        double erddl = getSigma( *HitMdc, a()[4], 0, ddl);
        double erddr = getSigma( *HitMdc, a()[4], 1, ddr);

        double dmeas2[2] = { 0.1*ddl, 0.1*ddr }; // millimeter to centimeter
        double er_dmeas2[2] = {0., 0.};
        if(resolflag_ == 1) { 
                er_dmeas2[0] = 0.1*erddl;
                er_dmeas2[1] = 0.1*erddr;
        }else if(resolflag_ == 0)  
        {
        }

        // Left hypothesis :
        if (lr == -1) {
                double er_dmeasL, dmeasL;
                if(Tof_correc_) {
                        dmeasL = (-1.0)*dmeas2[0];
                        er_dmeasL = er_dmeas2[0];
                } else {
                        dmeasL = (-1.0)*fabs((*HitMdc).dist()[0]);
                        er_dmeasL = (*HitMdc).erdist()[0];
                }


                //if(layerid < 4)  er_dmeasL*=2.0;

                double AL = 1 / ((v_H_T_ea_v_H)[0] + er_dmeasL*er_dmeasL);
                eaNew.assign(ea - ea_v_H * AL * ea_v_HT);
                double RkL = 1 - (v_H_T_ea_v_H)[0] * AL;
                if(0. == RkL) RkL = 1.e-4;

                HepVector diffL = ea_v_H * AL * (dmeasL - estim);
                aNew = v_a + diffL;
                double sigL = (dmeasL - (v_HT * aNew)[0]);
                dchi2L =  (sigL*sigL) /  (RkL * er_dmeasL*er_dmeasL);
        } else if (lr == 1) {
                // Right hypothesis :

                double er_dmeasR, dmeasR;
                if(Tof_correc_) {
                        dmeasR = dmeas2[1];
                        er_dmeasR = er_dmeas2[1];
                } else {
                        dmeasR = fabs((*HitMdc).dist()[1]);
                        er_dmeasR = (*HitMdc).erdist()[1];
                }


                //if(layerid < 4)  er_dmeasR*=2.0;


                double AR = 1 / ((v_H_T_ea_v_H)[0] + er_dmeasR*er_dmeasR);
                eaNew.assign(ea - ea_v_H * AR * ea_v_HT);
                double RkR = 1 - (v_H_T_ea_v_H)[0] * AR;
                if(0. == RkR) RkR = 1.e-4;

                HepVector diffR = ea_v_H * AR * (dmeasR - estim);
                aNew = v_a + diffR;
                double sigR = (dmeasR- (v_HT * aNew)[0]);
                dchi2R =  (sigR*sigR) /  (RkR * er_dmeasR*er_dmeasR);
        }

        // Update Kalman result :
#ifdef YDEBUG
        cout<<"in smoother_Mdc: lr= "<<lr<<" a: "<<v_a<<" a_NEW: "<<aNew<<endl;
#endif
        double dchi2_loc(0);
        if ((dchi2R < dchi2cuts_calib[layerid] && lr == 1) || 
                        (dchi2L < dchi2cuts_calib[layerid] && lr == -1)) {

                ndf_back_++;
                flg = 1;
                int chge(1);
                if (!(aNew[2] && fabs(aNew[2]-a()[2]) < 1.0))  chge = 0;
                if (lr == 1) {
                        chiSq_back_ += dchi2R;
                        dchi2_loc = dchi2R;
                        dd = 0.1*ddr;
                        // if(debug_ ==4) std::cout<<"in  smoother "<<std::endl;

                } else if (lr == -1) {
                        chiSq_back_ += dchi2L;
                        dchi2_loc = dchi2L;
                        dd = -0.1*ddl;

                } 
                if (chge){
                        a(aNew);
                        Ea(eaNew);
                }

                seg.a_filt_bwd(aNew);
                seg.Ea_filt_bwd(eaNew);

                HepVector a_pre_fwd_temp=seg.a_pre_fwd();
                if( (a_pre_fwd_temp[1]-seg.a_pre_bwd()[1]) > 3. * M_PI /2.)  a_pre_fwd_temp[1]-= M_PI2;
                if( (a_pre_fwd_temp[1]-seg.a_pre_bwd()[1]) < -3. * M_PI /2. )  a_pre_fwd_temp[1]+= M_PI2;

                seg.a_pre_fwd(a_pre_fwd_temp);

                HepVector a_pre_filt_temp=seg.a_filt_fwd();
                if( (a_pre_filt_temp[1]-seg.a_pre_bwd()[1]) > 3. * M_PI /2. )  a_pre_filt_temp[1] -= M_PI2;
                if( (a_pre_filt_temp[1]-seg.a_pre_bwd()[1]) < -3. * M_PI /2.)  a_pre_filt_temp[1] += M_PI2;
                seg.a_filt_fwd(a_pre_filt_temp);

                /*   
                     KalFitTrack helixNew(pivot_work, aNew, eaNew, 0, 0, 0);
                     helixNew.pivot(ip);
                     a_temp = helixNew.a();
                     ea_temp = helixNew.Ea();
                     seg.Ea_filt_bwd(ea_temp);
                     seg.a_filt_bwd(a_temp);
                 */

                int inv;

                if(debug_ == 4){
                        std::cout<<"seg.Ea_pre_bwd().inverse(inv) ..."<<seg.Ea_pre_bwd().inverse(inv)<<std::endl;
                        std::cout<<"seg.Ea_pre_fwd().inverse(inv) ..."<<seg.Ea_pre_fwd().inverse(inv)<<std::endl;
                }

                HepSymMatrix  Ea_pre_comb = (seg.Ea_pre_bwd().inverse(inv)+seg.Ea_pre_fwd().inverse(inv)).inverse(inv);
                seg.Ea_exclude(Ea_pre_comb);


                if(debug_ == 4) {
                        std::cout<<"Ea_pre_comb_ ... "<<Ea_pre_comb<<std::endl;
                        std::cout<<"seg.a_pre_bwd()..."<<seg.a_pre_bwd()<<std::endl;
                        std::cout<<"seg.a_pre_fwd()..."<<seg.a_pre_fwd()<<std::endl;
                }
                HepVector  a_pre_comb = Ea_pre_comb*((seg.Ea_pre_bwd().inverse(inv))*seg.a_pre_bwd()+(seg.Ea_pre_fwd().inverse(inv))*seg.a_pre_fwd());
                seg.a_exclude(a_pre_comb);

                if(debug_ == 4) {
                        std::cout<<"seg.Ea_filt_fwd()..."<<seg.Ea_filt_fwd()<<std::endl;
                        std::cout<<"seg.Ea_filt_fwd().inverse(inv)..."<<seg.Ea_filt_fwd().inverse(inv)<<std::endl;
                }
                seg.Ea((seg.Ea_filt_fwd().inverse(inv)+seg.Ea_pre_bwd().inverse(inv)).inverse(inv));
                seg.Ea_include((seg.Ea_filt_fwd().inverse(inv)+seg.Ea_pre_bwd().inverse(inv)).inverse(inv));

                if(debug_ == 4) {
                        std::cout<<"seg.Ea() is ..."<<seg.Ea()<<std::endl;
                        std::cout<<"seg.Ea_filt_fwd().inverse(inv)*seg.a_filt_fwd() ..."<<seg.Ea_filt_fwd().inverse(inv)*seg.a_filt_fwd()<<std::endl;
                        std::cout<<"seg.Ea_pre_bwd().inverse(inv)*seg.a_pre_bwd() ... "<<seg.Ea_pre_bwd().inverse(inv)*seg.a_pre_bwd()<<std::endl;
                }

                seg.a(seg.Ea()*(seg.Ea_filt_fwd().inverse(inv)*seg.a_filt_fwd()+seg.Ea_pre_bwd().inverse(inv)*seg.a_pre_bwd()));
                seg.a_include(seg.Ea()*(seg.Ea_filt_fwd().inverse(inv)*seg.a_filt_fwd()+seg.Ea_pre_bwd().inverse(inv)*seg.a_pre_bwd()));

                if(inv != 0) {
                        //std::cout<<"ERROR OCCUR WHEN INVERSE MATRIX !"<<std::endl;
                }

                seg.residual_exclude(dd - (v_HT*a_pre_comb)[0]);
                seg.residual_include(dd - (v_HT*seg.a())[0]);
                seg.doca_exclude(fabs((v_HT*a_pre_comb)[0]));
                seg.doca_include(fabs((v_HT*seg.a())[0]));

                if(debug_ == 4){
                        std::cout<<"the dd in smoother_Mdc is "<<dd
                                <<" the (v_HT*a_pre_comb)[0] is "<<(v_HT*a_pre_comb)[0]<<std::endl;
                }

                //compare the two method to calculate the include doca value : 
                if(debug_ == 4){
                        std::cout<<"method 1 ..."<<sqrt(seg.a()[0]*seg.a()[0]+seg.a()[3]*seg.a()[3])<<std::endl;
                        std::cout<<"method 2 ..."<<fabs((v_HT*seg.a())[0])<<std::endl;
                }


                KalFitTrack helixNew1(pivot_work, seg.a(), seg.Ea(), 0, 0, 0);
                helixNew1.pivot(ip);
                HepVector    a_temp1  = helixNew1.a();
                HepSymMatrix ea_temp1 = helixNew1.Ea();
                seg.Ea(ea_temp1);
                seg.a(a_temp1);
                seg.Ea_include(ea_temp1);
                seg.a_include(a_temp1);

                KalFitTrack helixNew2(pivot_work, seg.a_exclude(), seg.Ea_exclude(), 0, 0, 0);
                helixNew2.pivot(ip);
                HepVector    a_temp2  = helixNew2.a();
                HepSymMatrix ea_temp2 = helixNew2.Ea();    
                seg.Ea_exclude(ea_temp2);                    
                seg.a_exclude(a_temp2);             

                seg.tof(tofest);
                seg.dd(dd);

        }
        return chiSq_back_;
}

// Smoothing procedure in Mdc calib
// RMK attention for the case chi2R = chi2L during forward filter !
double KalFitTrack::smoother_Mdc(KalFitHelixSeg& seg, Hep3Vector& meas,int& flg, int csmflag )
{


        HepPoint3D ip(0., 0., 0.);
        // attention! we should not to decide the left&right in the smoother process,
        // because we choose the left&right of hits from the filter process. 

        KalFitHitMdc* HitMdc = seg.HitMdc();
        double lr = HitMdc->LR();

        // For taking the propagation delay into account :
        int layerid  = (*HitMdc).wire().layer().layerId();
        int wire_ID = HitMdc->wire().geoID();
        double entrangle = HitMdc->rechitptr()->getEntra();

        if (wire_ID<0 || wire_ID>6796){ //bes
                std::cout << "KalFitTrack : wire_ID problem : " << wire_ID << std::endl;
                return 0;
        } 

        double x[3] ={pivot().x(), pivot().y(), pivot().z()};
        double pmom[3] ={momentum().x(), momentum().y(), momentum().z()};
        double dd(0.);
        double tofest(0);
        double phi = fmod(phi0() + M_PI4, M_PI2);
        double csf0 = cos(phi);
        double snf0 = (1. - csf0) * (1. + csf0);
        snf0 = sqrt((snf0 > 0.) ? snf0 : 0.);
        if(phi > M_PI) snf0 = - snf0;

        if (Tof_correc_) {
                Hep3Vector ip(0, 0, 0);
                Helix work = *(Helix*)this;
                work.ignoreErrorMatrix();
                work.pivot(ip);
                double phi_ip = work.phi0();
                if (fabs(phi - phi_ip) > M_PI) {
                        if (phi > phi_ip) phi -= 2 * M_PI;
                        else phi_ip -= 2 * M_PI;
                }
                double t = tanl();
                double l = fabs(radius() * (phi - phi_ip) * sqrt(1 + t * t));
                double pmag( sqrt( 1.0 + t*t ) / kappa());
                double mass_over_p( mass_ / pmag );
                double beta( 1.0 / sqrt( 1.0 + mass_over_p * mass_over_p ) );
                tofest = l / ( 29.9792458 * beta );
                if(csmflag==1 && (*HitMdc).wire().y()>0.)  tofest= -1. * tofest;
        }

        const HepSymMatrix& ea = Ea();
        const HepVector& v_a = a();



        HepPoint3D pivot_work = pivot();

        /*
           HepVector a_work = a();
           HepSymMatrix ea_work = Ea();

           KalFitTrack helix_work(pivot_work, a_work, ea_work, 0, 0, 0);
           helix_work.pivot(ip);

           HepVector a_temp = helix_work.a();
           HepSymMatrix ea_temp = helix_work.Ea();

           seg.Ea_pre_bwd(ea_temp);     
           seg.a_pre_bwd(a_temp);

         */

        seg.a_pre_bwd(a()); 
        seg.Ea_pre_bwd(Ea()); 

        double dchi2R(99999999), dchi2L(99999999);
        HepVector v_H(5, 0);
        v_H[0] =  -csf0 * meas.x() - snf0 * meas.y();
        v_H[3] =  -meas.z();
        HepMatrix v_HT = v_H.T();

        double estim = (v_HT * v_a)[0];
        HepVector ea_v_H = ea * v_H;  
        HepMatrix ea_v_HT = (ea_v_H).T();
        HepVector v_H_T_ea_v_H = v_HT * ea_v_H;
        HepSymMatrix eaNew(5);
        HepVector aNew(5);
        double time = (*HitMdc).tdc();

        //seg.dt(time);
        // if (Tof_correc_)
        // { 
        //  time -= tofest;
        //  seg.dt(time);
        // }
        // double dd = 1.0e-4 * 40.0 * time;
        // seg.dd(dd);

        double drifttime = getDriftTime(*HitMdc , tofest);
        seg.dt(drifttime);
        double ddl = getDriftDist(*HitMdc, drifttime, 0 );
        double ddr = getDriftDist(*HitMdc, drifttime, 1 );
        double erddl = getSigma( *HitMdc, a()[4], 0, ddl);
        double erddr = getSigma( *HitMdc, a()[4], 1, ddr);

        double dmeas2[2] = { 0.1*ddl, 0.1*ddr }; // millimeter to centimeter
        double er_dmeas2[2] = {0., 0.};
        if(resolflag_ == 1) { 
                er_dmeas2[0] = 0.1*erddl;
                er_dmeas2[1] = 0.1*erddr;
        }else if(resolflag_ == 0)  
        {
        }

        // Left hypothesis :
        if (lr == -1) {
                double er_dmeasL, dmeasL;
                if(Tof_correc_) {
                        dmeasL = (-1.0)*dmeas2[0];
                        er_dmeasL = er_dmeas2[0];
                } else {
                        dmeasL = (-1.0)*fabs((*HitMdc).dist()[0]);
                        er_dmeasL = (*HitMdc).erdist()[0];
                }


                //if(layerid < 4)  er_dmeasL*=2.0;

                double AL = 1 / ((v_H_T_ea_v_H)[0] + er_dmeasL*er_dmeasL);
                eaNew.assign(ea - ea_v_H * AL * ea_v_HT);
                double RkL = 1 - (v_H_T_ea_v_H)[0] * AL;
                if(0. == RkL) RkL = 1.e-4;

                HepVector diffL = ea_v_H * AL * (dmeasL - estim);
                aNew = v_a + diffL;
                double sigL = (dmeasL - (v_HT * aNew)[0]);
                dchi2L =  (sigL*sigL) /  (RkL * er_dmeasL*er_dmeasL);
        } else if (lr == 1) {
                // Right hypothesis :

                double er_dmeasR, dmeasR;
                if(Tof_correc_) {
                        dmeasR = dmeas2[1];
                        er_dmeasR = er_dmeas2[1];
                } else {
                        dmeasR = fabs((*HitMdc).dist()[1]);
                        er_dmeasR = (*HitMdc).erdist()[1];
                }


                //if(layerid < 4)  er_dmeasR*=2.0;


                double AR = 1 / ((v_H_T_ea_v_H)[0] + er_dmeasR*er_dmeasR);
                eaNew.assign(ea - ea_v_H * AR * ea_v_HT);
                double RkR = 1 - (v_H_T_ea_v_H)[0] * AR;
                if(0. == RkR) RkR = 1.e-4;

                HepVector diffR = ea_v_H * AR * (dmeasR - estim);
                aNew = v_a + diffR;
                double sigR = (dmeasR- (v_HT * aNew)[0]);
                dchi2R =  (sigR*sigR) /  (RkR * er_dmeasR*er_dmeasR);
        }

        // Update Kalman result :
#ifdef YDEBUG
        cout<<"in smoother_Mdc: lr= "<<lr<<" a: "<<v_a<<" a_NEW: "<<aNew<<endl;
#endif
        double dchi2_loc(0);
        if ((dchi2R < dchi2cuts_calib[layerid] && lr == 1) || 
                        (dchi2L < dchi2cuts_calib[layerid] && lr == -1)) {

                ndf_back_++;
                flg = 1;
                int chge(1);
                if (!(aNew[2] && fabs(aNew[2]-a()[2]) < 1.0))  chge = 0;
                if (lr == 1) {
                        chiSq_back_ += dchi2R;
                        dchi2_loc = dchi2R;
                        dd = 0.1*ddr;
                        // if(debug_ ==4) std::cout<<"in  smoother "<<std::endl;

                } else if (lr == -1) {
                        chiSq_back_ += dchi2L;
                        dchi2_loc = dchi2L;
                        dd = -0.1*ddl;

                } 
                if (chge){
                        a(aNew);
                        Ea(eaNew);
                }

                seg.a_filt_bwd(aNew);
                seg.Ea_filt_bwd(eaNew);

                HepVector a_pre_fwd_temp=seg.a_pre_fwd();
                if( (a_pre_fwd_temp[1]-seg.a_pre_bwd()[1]) > 3. * M_PI /2.)  a_pre_fwd_temp[1]-= M_PI2;
                if( (a_pre_fwd_temp[1]-seg.a_pre_bwd()[1]) < -3. * M_PI /2. )  a_pre_fwd_temp[1]+= M_PI2;

                seg.a_pre_fwd(a_pre_fwd_temp);

                HepVector a_pre_filt_temp=seg.a_filt_fwd();
                if( (a_pre_filt_temp[1]-seg.a_pre_bwd()[1]) > 3. * M_PI /2. )  a_pre_filt_temp[1] -= M_PI2;
                if( (a_pre_filt_temp[1]-seg.a_pre_bwd()[1]) < -3. * M_PI /2.)  a_pre_filt_temp[1] += M_PI2;
                seg.a_filt_fwd(a_pre_filt_temp);

                /*   
                     KalFitTrack helixNew(pivot_work, aNew, eaNew, 0, 0, 0);
                     helixNew.pivot(ip);
                     a_temp = helixNew.a();
                     ea_temp = helixNew.Ea();
                     seg.Ea_filt_bwd(ea_temp);
                     seg.a_filt_bwd(a_temp);
                 */

                int inv;

                if(debug_ == 4){
                        std::cout<<"seg.Ea_pre_bwd().inverse(inv) ..."<<seg.Ea_pre_bwd().inverse(inv)<<std::endl;
                        std::cout<<"seg.Ea_pre_fwd().inverse(inv) ..."<<seg.Ea_pre_fwd().inverse(inv)<<std::endl;
                }

                HepSymMatrix  Ea_pre_comb = (seg.Ea_pre_bwd().inverse(inv)+seg.Ea_pre_fwd().inverse(inv)).inverse(inv);
                seg.Ea_exclude(Ea_pre_comb);


                if(debug_ == 4) {
                        std::cout<<"Ea_pre_comb_ ... "<<Ea_pre_comb<<std::endl;
                        std::cout<<"seg.a_pre_bwd()..."<<seg.a_pre_bwd()<<std::endl;
                        std::cout<<"seg.a_pre_fwd()..."<<seg.a_pre_fwd()<<std::endl;
                }
                HepVector  a_pre_comb = Ea_pre_comb*((seg.Ea_pre_bwd().inverse(inv))*seg.a_pre_bwd()+(seg.Ea_pre_fwd().inverse(inv))*seg.a_pre_fwd());
                seg.a_exclude(a_pre_comb);


                if(debug_ == 4) {
                        std::cout<<"seg.Ea_filt_fwd()..."<<seg.Ea_filt_fwd()<<std::endl;
                        std::cout<<"seg.Ea_filt_fwd().inverse(inv)..."<<seg.Ea_filt_fwd().inverse(inv)<<std::endl;
                }
                seg.Ea((seg.Ea_filt_fwd().inverse(inv)+seg.Ea_pre_bwd().inverse(inv)).inverse(inv));
                seg.Ea_include((seg.Ea_filt_fwd().inverse(inv)+seg.Ea_pre_bwd().inverse(inv)).inverse(inv));

                if(debug_ == 4) {
                        std::cout<<"seg.Ea() is ..."<<seg.Ea()<<std::endl;
                        std::cout<<"seg.Ea_filt_fwd().inverse(inv)*seg.a_filt_fwd() ..."<<seg.Ea_filt_fwd().inverse(inv)*seg.a_filt_fwd()<<std::endl;
                        std::cout<<"seg.Ea_pre_bwd().inverse(inv)*seg.a_pre_bwd() ... "<<seg.Ea_pre_bwd().inverse(inv)*seg.a_pre_bwd()<<std::endl;
                }

                seg.a(seg.Ea()*(seg.Ea_filt_fwd().inverse(inv)*seg.a_filt_fwd()+seg.Ea_pre_bwd().inverse(inv)*seg.a_pre_bwd()));
                seg.a_include(seg.Ea()*(seg.Ea_filt_fwd().inverse(inv)*seg.a_filt_fwd()+seg.Ea_pre_bwd().inverse(inv)*seg.a_pre_bwd()));

                if(inv != 0) {
                        //std::cout<<"ERROR OCCUR WHEN INVERSE MATRIX !"<<std::endl;
                }

                seg.residual_exclude(dd - (v_HT*a_pre_comb)[0]);
                seg.residual_include(dd - (v_HT*seg.a())[0]);
                seg.doca_exclude(fabs((v_HT*a_pre_comb)[0]));
                seg.doca_include(fabs((v_HT*seg.a())[0]));

                if(debug_ == 4){
                        std::cout<<"the dd in smoother_Mdc is "<<dd
                                <<" the (v_HT*a_pre_comb)[0] is "<<(v_HT*a_pre_comb)[0]<<std::endl;
                }

                //compare the two method to calculate the include doca value : 
                if(debug_ == 4){
                        std::cout<<"method 1 ..."<<sqrt(seg.a()[0]*seg.a()[0]+seg.a()[3]*seg.a()[3])<<std::endl;
                        std::cout<<"method 2 ..."<<fabs((v_HT*seg.a())[0])<<std::endl;
                }


                KalFitTrack helixNew1(pivot_work, seg.a(), seg.Ea(), 0, 0, 0);
                helixNew1.pivot(ip);
                HepVector    a_temp1  = helixNew1.a();
                HepSymMatrix ea_temp1 = helixNew1.Ea();
                seg.Ea(ea_temp1);
                seg.a(a_temp1);
                seg.Ea_include(ea_temp1);
                seg.a_include(a_temp1);

                KalFitTrack helixNew2(pivot_work, seg.a_exclude(), seg.Ea_exclude(), 0, 0, 0);
                helixNew2.pivot(ip);
                HepVector    a_temp2  = helixNew2.a();
                HepSymMatrix ea_temp2 = helixNew2.Ea();    
                seg.Ea_exclude(ea_temp2);                    
                seg.a_exclude(a_temp2);             

                seg.tof(tofest);
                seg.dd(dd);

        }
        return chiSq_back_;
}





double KalFitTrack::filter(double v_m, const HepVector& m_H,
                double v_d, double m_V)
{
        HepMatrix m_HT = m_H.T();
        HepPoint3D x0 = x(0);
        HepVector v_x(3);
        v_x[0] = x0.x();
        v_x[1] = x0.y();
        v_x[2] = x0.z();
        HepMatrix m_X = delXDelA(0);
        HepMatrix m_XT = m_X.T();
        HepMatrix m_C = m_X * Ea() * m_XT;
        //int err;
        HepVector m_K = 1 / (m_V + (m_HT * m_C * m_H)[0]) * m_H;
        HepVector v_a = a();
        HepSymMatrix ea = Ea();
        HepMatrix mXTmK = m_XT * m_K;
        double v_r = v_m - v_d - (m_HT * v_x)[0];
        v_a += ea * mXTmK * v_r;
        a(v_a);
        ea.assign(ea - ea * mXTmK * m_HT * m_X * ea);
        Ea(ea);
        // Record last hit included parameters :
        update_last();
        HepMatrix mCmK = m_C * m_K;
        v_x += mCmK * v_r;
        m_C -= mCmK * m_HT * m_C;
        v_r = v_m - v_d - (m_HT * v_x)[0];
        double m_R = m_V - (m_HT * m_C * m_H)[0];
        double chiSq = v_r * v_r / m_R;
        chiSq_ += chiSq;
        return chiSq;
}


void KalFitTrack::path_add(double path)
{
        pathip_ += path;
        tof(path);
}


void KalFitTrack::addPathSM(double path){
        pathSM_ += path;
}


void KalFitTrack::addTofSM(double time){
        tof2_ += time;
} 


void KalFitTrack::fiTerm(double fi){
        fiTerm_ = fi;
}


void KalFitTrack::tof(double path)
{
        double light_speed( 29.9792458 );     // light speed in cm/nsec
        double t = tanl();
        double pmag( sqrt( 1.0 + t*t ) / kappa());
        if (pmag !=0) {
                double mass_over_p( mass_ / pmag );
                double beta( 1.0 / sqrt( 1.0 + mass_over_p * mass_over_p ) );
                tof_ += path / ( light_speed * beta );
        }

        if (tofall_) {
                if (p_kaon_ > 0){
                        double massk_over_p( MASS[3] / p_kaon_ );
                        double beta_kaon( 1.0 / sqrt( 1.0 + massk_over_p * massk_over_p ) );
                        tof_kaon_ += path / (light_speed * beta_kaon);
                }
                if (p_proton_ > 0){
                        double massp_over_p( MASS[4] / p_proton_ );
                        double beta_proton( 1.0 / sqrt( 1.0 + massp_over_p * massp_over_p ) );
                        tof_proton_ += path / (light_speed * beta_proton);
                }
        }
}

double KalFitTrack::radius_numf(void) const {

        double Bz(Bznom_);
        //std::cout<<"Bz: "<<Bz<<std::endl;
        if (numf_ > 10){
                double dr    = a()[0];
                double phi0  = a()[1];
                double dz    = a()[3];
                double phi = fmod(phi0 + M_PI4, M_PI2);
                double csf0 = cos(phi);
                double snf0 = (1. - csf0) * (1. + csf0);
                snf0 = sqrt((snf0 > 0.) ? snf0 : 0.);
                if(phi > M_PI) snf0 = - snf0;
                //XYZPoint
                HepPoint3D x0((pivot().x() + dr*csf0),
                                (pivot().y() + dr*snf0),
                                (pivot().z() + dz));

                //XYZVector b;
                HepVector3D b;

                //std::cout<<"b: "<<b<<std::endl;

                MFSvc_->fieldVector(10.*x0, b);

                //std::cout<<"b: "<<b<<std::endl;


                b = 10000.*b;
                Bz = b.z();
        }
        if (Bz == 0)
                Bz = Bznom_;
        double ALPHA_loc = 10000./2.99792458/Bz;
        return ALPHA_loc / a()[2];
}

const HepPoint3D &
KalFitTrack::pivot_numf(const HepPoint3D & newPivot) {

        int nstep(1);
        HepPoint3D  delta_x((newPivot-pivot()).x()/double(inner_steps_),
                        (newPivot-pivot()).y()/double(inner_steps_),
                        (newPivot-pivot()).z()/double(inner_steps_));
        int i = 1;

        while (i <= inner_steps_) {
                HepPoint3D nextPivot(pivot()+delta_x);
                double xnp(nextPivot.x()), ynp(nextPivot.y()), znp(nextPivot.z());  
                HepSymMatrix Ea_now = Ea();
                HepPoint3D piv(pivot());
                double xp(piv.x()), yp(piv.y()), zp(piv.z());  
                double dr    = a()[0];
                double phi0  = a()[1];
                double kappa = a()[2];
                double dz    = a()[3];
                double tanl  = a()[4];
                double m_rad(0);
                if (numfcor_ == 1)
                        m_rad = radius_numf();
                else
                        m_rad = radius();
                double rdr = dr + m_rad;
                double phi = fmod(phi0 + M_PI4, M_PI2);
                double csf0 = cos(phi);
                double snf0 = (1. - csf0) * (1. + csf0);
                snf0 = sqrt((snf0 > 0.) ? snf0 : 0.);
                if(phi > M_PI) snf0 = - snf0;

                double xc = xp + rdr * csf0;
                double yc = yp + rdr * snf0;
                double csf = (xc - xnp) / m_rad;
                double snf = (yc - ynp) / m_rad;
                double anrm = sqrt(csf * csf + snf * snf);
                csf /= anrm;
                snf /= anrm;
                phi = atan2(snf, csf);
                double phid = fmod(phi - phi0 + M_PI8, M_PI2);
                if(phid > M_PI) phid = phid - M_PI2;
                double drp = (xp + dr * csf0 + m_rad * (csf0 - csf) - xnp)
                        * csf
                        + (yp + dr * snf0 + m_rad * (snf0 - snf) - ynp) * snf;
                double dzp = zp + dz - m_rad * tanl * phid - znp;

                HepVector ap(5);
                ap[0] = drp;
                ap[1] = fmod(phi + M_PI4, M_PI2);
                ap[2] = kappa;
                ap[3] = dzp;
                ap[4] = tanl;

                // Modification due to non uniform magnetic field :
                if (numf_ > 10) {

                        Hep3Vector x1(xp + dr*csf0, yp + dr*snf0, zp + dz);
                        double csf0p = cos(ap[1]);
                        double snf0p = (1. - csf0p) * (1. + csf0p);
                        snf0p = sqrt((snf0p > 0.) ? snf0p : 0.);
                        if(ap[1] > M_PI) snf0p = - snf0p;

                        Hep3Vector x2(xnp + drp*csf0p,
                                        ynp + drp*snf0p,
                                        znp + dzp);
                        Hep3Vector dist((x1 - x2).x()/100.0,
                                        (x1 - x2).y()/100.0,
                                        (x1 - x2).z()/100.0);
                        HepPoint3D middlebis((x1.x() + x2.x())/2,
                                        (x1.y() + x2.y())/2,
                                        (x1.z() + x2.z())/2);
                        HepVector3D field;
                        MFSvc_->fieldVector(10.*middlebis,field); 
                        field = 1000.*field;
                        Hep3Vector dB(field.x(),
                                        field.y(),
                                        (field.z()-0.1*Bznom_));
                        if (field.z()) {
                                double akappa(fabs(kappa));
                                double sign = kappa/akappa;
                                HepVector dp(3);
                                dp = 0.299792458 * sign * dB.cross(dist);
                                HepVector dhp(3);
                                dhp[0] = -akappa*(dp[0]*csf0p+dp[1]*snf0p);
                                dhp[1] = kappa*akappa*(dp[0]*snf0p-dp[1]*csf0p);
                                dhp[2] = dp[2]*akappa+dhp[1]*tanl/kappa;
                                if (numfcor_ == 0){
                                        ap[1] += dhp[0];
                                }
                                ap[2] += dhp[1];
                                ap[4] += dhp[2];
                        }
                }
                HepMatrix m_del = delApDelA(ap);
                Ea_now.assign(m_del * Ea_now * m_del.T());
                pivot(nextPivot);
                a(ap);
                Ea(Ea_now);
                i++;
        }
        return newPivot;
}

const HepPoint3D &
KalFitTrack::pivot_numf(const HepPoint3D & newPivot, double & pathl) {

        Helix tracktest = *(Helix*)this;
        tracktest.ignoreErrorMatrix();
        double tl  = a()[4];
        double th = 90.0 - 180.0*M_1_PI*atan( tl );
        /*
           int nstep(1);
           if (steplev_ == 1)
           nstep = 3;
           else if (steplev_ == 2 && (th > 140 || th <45))
           if ((pivot()-newPivot).mag()<3.)
           nstep = 3;
           else
           nstep = 6;
         */
        Hep3Vector delta_x((newPivot-pivot()).x()/double(outer_steps_),
                        (newPivot-pivot()).y()/double(outer_steps_),
                        (newPivot-pivot()).z()/double(outer_steps_));
        int i = 1;

        while (i <= outer_steps_) {
                HepPoint3D nextPivot(pivot()+delta_x);
                double xnp(nextPivot.x()), ynp(nextPivot.y()), znp(nextPivot.z());  

                HepSymMatrix Ea_now = Ea();
                HepPoint3D piv(pivot());
                double xp(piv.x()), yp(piv.y()), zp(piv.z());  

                double dr    = a()[0];
                double phi0  = a()[1];
                double kappa = a()[2];
                double dz    = a()[3];
                double tanl  = a()[4];

                double m_rad(0);
                m_rad = radius();

                double rdr = dr + m_rad;
                double phi = fmod(phi0 + M_PI4, M_PI2);
                double csf0 = cos(phi);
                double snf0 = (1. - csf0) * (1. + csf0);
                snf0 = sqrt((snf0 > 0.) ? snf0 : 0.);
                if(phi > M_PI) snf0 = - snf0;

                double xc = xp + rdr * csf0;
                double yc = yp + rdr * snf0;
                double csf = (xc - xnp) / m_rad;
                double snf = (yc - ynp) / m_rad;
                double anrm = sqrt(csf * csf + snf * snf);
                csf /= anrm;
                snf /= anrm;
                phi = atan2(snf, csf);
                double phid = fmod(phi - phi0 + M_PI8, M_PI2);
                if(phid > M_PI) phid = phid - M_PI2;
                double drp = (xp + dr * csf0 + m_rad * (csf0 - csf) - xnp)
                        * csf
                        + (yp + dr * snf0 + m_rad * (snf0 - snf) - ynp) * snf;
                double dzp = zp + dz - m_rad * tanl * phid - znp;

                HepVector ap(5);
                ap[0] = drp;
                ap[1] = fmod(phi + M_PI4, M_PI2);
                ap[2] = kappa;
                ap[3] = dzp;
                ap[4] = tanl;

                //std::cout<<" numf_: "<<numf_<<std::endl;

                // Modification due to non uniform magnetic field :
                if (numf_ > 10) {

                        Hep3Vector x1(xp + dr*csf0, yp + dr*snf0, zp + dz);

                        double csf0p = cos(ap[1]);
                        double snf0p = (1. - csf0p) * (1. + csf0p);
                        snf0p = sqrt((snf0p > 0.) ? snf0p : 0.);
                        if(ap[1] > M_PI) snf0p = - snf0p;

                        Hep3Vector x2(xnp + drp*csf0p,
                                        ynp + drp*snf0p,
                                        znp + dzp);

                        Hep3Vector dist((x1 - x2).x()/100.0,
                                        (x1 - x2).y()/100.0,
                                        (x1 - x2).z()/100.0);

                        HepPoint3D middlebis((x1.x() + x2.x())/2,
                                        (x1.y() + x2.y())/2,
                                        (x1.z() + x2.z())/2);

                        HepVector3D field;
                        MFSvc_->fieldVector(10.*middlebis,field);
                        field = 1000.*field;

                        //std::cout<<"B field: "<<field<<std::endl;
                        Hep3Vector dB(field.x(),
                                        field.y(),
                                        (field.z()-0.1*Bznom_));


                        //std::cout<<" dB: "<<dB<<std::endl;


                        if (field.z()) {
                                double akappa(fabs(kappa));
                                double sign = kappa/akappa;
                                HepVector dp(3);
                                dp = 0.299792458 * sign * dB.cross(dist);

                                //std::cout<<"dp: "<<dp<<std::endl;

                                HepVector dhp(3);
                                dhp[0] = -akappa*(dp[0]*csf0p+dp[1]*snf0p);
                                dhp[1] = kappa*akappa*(dp[0]*snf0p-dp[1]*csf0p);
                                dhp[2] = dp[2]*akappa+dhp[1]*tanl/kappa;


                                //std::cout<<"dhp: "<<dhp<<std::endl;


                                ap[1] += dhp[0];
                                ap[2] += dhp[1];
                                ap[4] += dhp[2];
                        }
                }
                HepMatrix m_del = delApDelA(ap);
                Ea_now.assign(m_del * Ea_now * m_del.T());
                pivot(nextPivot);
                a(ap);
                Ea(Ea_now);
                i++;

                //std::cout<<" a: "<<a()<<std::endl;
        }

        // Estimation of the path length :
        double tanl_0(tracktest.a()[4]);
        double phi0_0(tracktest.a()[1]);
        double radius_0(tracktest.radius());
        tracktest.pivot(newPivot);

        double phi0_1 = tracktest.a()[1];
        if (fabs(phi0_1 - phi0_0) > M_PI) {
                if (phi0_1 > phi0_0) phi0_1 -= 2 * M_PI;
                else phi0_0 -= 2 * M_PI;
        }  
        if(phi0_1 == phi0_0) phi0_1 = phi0_0 + 1.e-10;
        pathl = fabs(radius_0 * (phi0_1 - phi0_0)
                        * sqrt(1 + tanl_0 * tanl_0));
        return newPivot;
}

// function to calculate the path length in the layer
/*
   double KalFitTrack::PathL(int layer){
   HepPoint3D piv(pivot());
   double phi0  = a()[1];
   double kappa = a()[2];
   double tanl  = a()[4];

#ifdef YDEBUG
cout<<"helix "<<a()<<endl;
#endif
// Choose the local field !!
double Bz(Bznom_);
if (numf_ > 10){
HepVector3D b;
MFSvc_->fieldVector(10.*piv, b);   
b = 10000.*b;
Bz=b.z();
}
double ALPHA_loc = 10000./2.99792458/Bz;
int charge = ( kappa >= 0 )? 1 : -1;
double rho = ALPHA_loc/kappa;
double pt = fabs( 1.0/kappa );
double lambda = 180.0*M_1_PI*atan( tanl );
double theta = 90.0 - lambda;
theta = 2.0*M_PI*theta/360.;
double phi = fmod(phi0 + M_PI4, M_PI2);
double csf0 = cos(phi);
double snf0 = (1. - csf0) * (1. + csf0);
snf0 = sqrt((snf0 > 0.) ? snf0 : 0.);
if(phi > M_PI) snf0 = - snf0;

double x_c = piv.x() + ( dr() + rho )*csf0;
double y_c = piv.y() + ( dr() + rho )*snf0;
double z_c = piv.z() + dz();
HepPoint3D ccenter(x_c, y_c, 0);
double m_c_perp(ccenter.perp());
Hep3Vector m_c_unit(((CLHEP::Hep3Vector)ccenter).unit());
#ifdef YDEBUG
cout<<"rho=..."<<rho<<endl;
cout<<"ccenter "<<ccenter<<" m_c_unit "<<m_c_unit<<endl;
#endif
//phi resion estimation
double phi_io[2];
HepPoint3D IO = charge*m_c_unit;
double dphi0 = fmod( IO.phi()+4*M_PI,2*M_PI ) - phi;
if( dphi0 > M_PI ) dphi0 -= 2*M_PI;
else if( dphi0 < -M_PI ) dphi0 += 2*M_PI;
phi_io[0] = -(1+charge)*M_PI_2 - charge*dphi0;
phi_io[1] = phi_io[0]+1.5*M_PI;
double phi_in(0); /// for debug

double m_crio[2];
double m_zb, m_zf;
int m_div;

// retrieve Mdc  geometry information
// IMdcGeomSvc* geosvc;
StatusCode sc= Gaudi::svcLocator()->service("MdcGeomSvc", geosvc);
if(sc==StatusCode::FAILURE) cout << "GeoSvc failing!!!!!!!SC=" << sc << endl; 

MdcGeomSvc* geomsvc = dynamic_cast<MdcGeomSvc*>(iGeomSvc_);
if(!geomsvc){
std::cout<<"ERROR OCCUR when dynamic_cast in KalFitTrack.cxx !!"<<std::endl;
}
double rcsiz1 = geomsvc->Layer(layer)->RCSiz1();
double rcsiz2 = geomsvc->Layer(layer)->RCSiz2();
double rlay = geomsvc->Layer(layer)->Radius();
m_zb = 0.5*(geomsvc->Layer(layer)->Length());
m_zf = -0.5*(geomsvc->Layer(layer)->Length());

m_crio[0] = rlay - rcsiz1;
m_crio[1] = rlay + rcsiz2;
//conversion of the units(mm=>cm)
m_crio[0] = 0.1*m_crio[0];
m_crio[1] = 0.1*m_crio[1];
m_zb = 0.1*m_zb;
m_zf = 0.1*m_zf;

int sign = 1;  ///assumed the first half circle
int epflag[2];
Hep3Vector iocand;
Hep3Vector cell_IO[2];
double dphi;
for( int i = 0; i < 2; i++ ){
        double cos_alpha = m_c_perp*m_c_perp + m_crio[i]*m_crio[i] - rho*rho;
        cos_alpha = 0.5*cos_alpha/( m_c_perp*m_crio[i] );
        double Calpha =  acos( cos_alpha );
        epflag[i] = 0;
        iocand = m_c_unit;
        iocand.rotateZ( charge*sign*Calpha );
        iocand *= m_crio[i];
        double xx = iocand.x() - x_c;
        double yy = iocand.y() - y_c;
        dphi = atan2(yy, xx) - phi0 - M_PI_2*(1+charge);
        dphi = fmod( dphi + 8.0*M_PI, 2*M_PI );

        if( dphi < phi_io[0] ){
                dphi += 2*M_PI;
        }
        else if( phi_io[1] < dphi ){
                dphi -= 2*M_PI;
        }


        Hep3Vector zvector( 0., 0., z_c-rho*dphi*tanl);

        double xcio, ycio, phip;
        double delrin = 2.0;
        if( m_zf-delrin > zvector.z() ){
                phip = z_c - m_zb + delrin;
                phip = phip/( rho*tanl );
                phip = phip + phi0;
                xcio = x_c - rho*cos( phip );
                ycio = y_c - rho*sin( phip );
                cell_IO[i].setX( xcio );
                cell_IO[i].setY( ycio );
                cell_IO[i].setZ( m_zb+delrin );
                epflag[i] = 1;
        }
        else if( m_zb+delrin < zvector.z() ){
                phip = z_c - m_zf-delrin;
                phip = phip/( rho*tanl );
                phip = phip + phi0;
                xcio = x_c - rho*cos( phip );
                ycio = y_c - rho*sin( phip );
                cell_IO[i].setX( xcio );
                cell_IO[i].setY( ycio );
                cell_IO[i].setZ( m_zf-delrin );
                epflag[i] = 1;
        }
        else{
                cell_IO[i] = iocand;
                cell_IO[i] += zvector;
        }
        if( i == 0 ) phi_in = dphi;
}
// path length estimation
// phi calculation
Hep3Vector cl = cell_IO[1] - cell_IO[0];
#ifdef YDEBUG
cout<<"cell_IO[0] "<<cell_IO[0]<<" cell_IO[1] "<<cell_IO[1]<<" cl "<<cl<<endl; 
#endif

double ch_theta;
double ch_dphi;
double ch_ltrk = 0;
double ch_ltrk_rp = 0;
ch_dphi = cl.perp()*0.5/(ALPHA_loc*pt);
ch_dphi = 2.0 * asin( ch_dphi );
ch_ltrk_rp = ch_dphi*ALPHA_loc*pt;
#ifdef YDEBUG
cout<<"ch_dphi "<<ch_dphi<<" ch_ltrk_rp "<<ch_ltrk_rp<<" cl.z "<<cl.z()<<endl; 
#endif
ch_ltrk = sqrt( ch_ltrk_rp*ch_ltrk_rp + cl.z()*cl.z() );
ch_theta = cl.theta();

if( ch_theta < theta*0.85 || 1.15*theta < ch_theta)
        ch_ltrk *= -1; /// miss calculation

if( epflag[0] == 1 || epflag [1] == 1 )
        ch_ltrk *= -1;  /// invalid resion for dE/dx or outside of Mdc

        mom_[layer] = momentum( phi_in );
        PathL_[layer] = ch_ltrk;
#ifdef YDEBUG
        cout<<"ch_ltrk "<<ch_ltrk<<endl; 
#endif
        return ch_ltrk;
        }
*/


void KalFitTrack::update_bit(int i){
        int j(0);
        if (i < 31){
                j = (int) pow(2.,i);
                if (!(pat1_ & j))
                        pat1_ = pat1_ | j;
        } else if (i < 50) {
                j = (int) pow(2.,(i-31));
                if (!(pat2_ & j))
                        pat2_ = pat2_ | j;
        }
}

int KalFitTrack::nmass(void){  return NMASS;}
double KalFitTrack::mass(int i){  return MASS[i];}
void KalFitTrack::chgmass(int i){
        mass_=MASS[i];
        l_mass_=i;
}

void KalFitTrack::lead(int i){ lead_ = i;}
int KalFitTrack::lead(void){ return lead_;}
void KalFitTrack::back(int i){ back_ = i;}
int KalFitTrack::back(void){ return back_;}
void KalFitTrack::resol(int i){ resolflag_ = i;}
int KalFitTrack::resol(void){ return resolflag_;}
void KalFitTrack::numf(int i){ numf_ = i;}
int KalFitTrack::numf(void){ return numf_;}
void KalFitTrack::LR(int x){ LR_ = x;}
void KalFitTrack::HitsMdc(vector<KalFitHitMdc>& lh){ HitsMdc_ = lh;}
void KalFitTrack::appendHitsMdc(KalFitHitMdc h){ HitsMdc_.push_back(h);}

/* 
   This member function is in charge of the forward filter,
   for the tof correction of the drift distance in the case of cosmic rays
   if way=1, it's a down track means same as track in collision data,
   if way=-1, it's a up track !
 */

double KalFitTrack::update_hits(KalFitHitMdc & HitMdc, int inext, Hep3Vector& meas, int way, double& dchi2out, double& dtrack,double& dtracknew, double& dtdc, int csmflag) {

        double lr = HitMdc.LR();
        //std::cout<<" in update_hits: lr= " << lr <<std::endl;
        // For taking the propagation delay into account :
        const KalFitWire& Wire = HitMdc.wire();
        int wire_ID = Wire.geoID();
        int layerid = HitMdc.wire().layer().layerId();
        //std::cout<<" when in layer: "<<layerid<<std::endl;
        double entrangle = HitMdc.rechitptr()->getEntra();

        if (wire_ID<0 || wire_ID>6796){  //bes
                std::cout << " KalFitTrack: wire_ID problem : " << wire_ID 
                        << std::endl;
                return 0;
        } 
        int flag_ster = Wire.stereo();
        double x[3] ={pivot().x(), pivot().y(), pivot().z()};
        double pmom[3] ={momentum().x(), momentum().y(), momentum().z()};
        double tofest(0);
        double phi = fmod(phi0() + M_PI4, M_PI2);
        double csf0 = cos(phi);
        double snf0 = (1. - csf0) * (1. + csf0);
        snf0 = sqrt((snf0 > 0.) ? snf0 : 0.);
        if(phi > M_PI) snf0 = - snf0;
        if (Tof_correc_){
                double t = tanl();
                double dphi(0);

                Helix work = *(Helix*)this;
                work.ignoreErrorMatrix();
                double phi_ip(0);
                Hep3Vector ip(0, 0, 0);
                work.pivot(ip);
                phi_ip = work.phi0();
                if (fabs(phi - phi_ip) > M_PI) {
                        if (phi > phi_ip) phi -= 2 * M_PI;
                        else phi_ip -= 2 * M_PI;
                }
                dphi = phi - phi_ip;
                double l = fabs(radius() * dphi * sqrt(1 + t * t));
                double pmag( sqrt( 1.0 + t*t ) / kappa());
                double mass_over_p( mass_ / pmag );
                double beta( 1.0 / sqrt( 1.0 + mass_over_p * mass_over_p ) );
                tofest = l / ( 29.9792458 * beta );
                if(csmflag==1 && HitMdc.wire().y()>0.)  tofest= -1. * tofest;
        }

        const HepSymMatrix& ea = Ea();
        const HepVector& v_a = a();
        double dchi2R(99999999), dchi2L(99999999);

        HepVector v_H(5, 0);
        v_H[0] =  -csf0 * meas.x() - snf0 * meas.y();
        v_H[3] =  -meas.z();
        HepMatrix v_HT = v_H.T();

        double estim = (v_HT * v_a)[0];
        dtrack = estim;
        // std::cout<<" in update_hits estim is  "<<estim<<std::endl;
        HepVector ea_v_H = ea * v_H;
        HepMatrix ea_v_HT = (ea_v_H).T();
        HepVector v_H_T_ea_v_H = v_HT * ea_v_H;

        HepSymMatrix eaNewL(5), eaNewR(5);
        HepVector aNewL(5), aNewR(5);

        double drifttime = getDriftTime(HitMdc , tofest);
        double ddl = getDriftDist(HitMdc, drifttime, 0 );
        double ddr = getDriftDist(HitMdc, drifttime, 1 );
        double erddl = getSigma( HitMdc, a()[4], 0, ddl);
        double erddr = getSigma( HitMdc, a()[4], 1, ddr);

        if(debug_ == 4) {
                std::cout<<"drifttime in update_hits() for ananlysis is  "<<drifttime<<std::endl;
                std::cout<<"ddl in update_hits() for ananlysis is  "<<ddl<<std::endl;
                std::cout<<"ddr in update_hits() for ananlysis is  "<<ddr<<std::endl;
                std::cout<<"erddl in update_hits() for ananlysis is  "<<erddl<<std::endl;
                std::cout<<"erddr in update_hits() for ananlysis is  "<<erddr<<std::endl;
        }

        //the following 3 line : tempory
        double dmeas2[2] = { 0.1*ddl, 0.1*ddr };  // millimeter to centimeter
        double er_dmeas2[2] = {0.,0.};
        if(resolflag_ == 1) { 
                er_dmeas2[0] = 0.1*erddl;
                er_dmeas2[1] = 0.1*erddr;
        } 
        else if(resolflag_ == 0) {
                //   int layid = HitMdc.wire().layer().layerId();
                //   double sigma = getSigma(layid, dd);
                //   er_dmeas2[0] = er_dmeas2[1] = sigma;
        }

        // Left hypothesis :
        if ((LR_==0 && lr != 1.0) || 
                        (LR_==1 && lr == -1.0)) {
                double er_dmeasL, dmeasL;
                if(Tof_correc_) {
                        dmeasL = (-1.0)*fabs(dmeas2[0]);
                        er_dmeasL = er_dmeas2[0];
                } else {
                        dmeasL = (-1.0)*fabs(HitMdc.dist()[0]);
                        er_dmeasL = HitMdc.erdist()[0];
                }
                dtdc = dmeasL;
                double AL = 1 / ((v_H_T_ea_v_H)[0] + er_dmeasL*er_dmeasL);
                eaNewL.assign(ea - ea_v_H * AL * ea_v_HT);
                double RkL = 1 - (v_H_T_ea_v_H)[0] * AL;
                if(debug_ == 4){
                        std::cout<<" ea_v_H * AL * ea_v_HT is: "<<ea_v_H * AL * ea_v_HT<<std::endl;
                        std::cout<<" v_H is: "<<v_H<<" Ea is: "<<ea<<" v_H_T_ea_v_H is: "<<v_H_T_ea_v_H<<std::endl;
                        std::cout<<" AL is: "<<AL<<" (v_H_T_ea_v_H)[0] * AL is: "<<(v_H_T_ea_v_H)[0]*AL<<std::endl;
                }

                if(0. == RkL) RkL = 1.e-4;

                HepVector diffL = ea_v_H * AL * (dmeasL - estim);
                aNewL = v_a + diffL;
                double sigL = dmeasL -(v_HT * aNewL)[0];
                dtracknew = (v_HT * aNewL)[0];
                dchi2L = (sigL * sigL)/(RkL * er_dmeasL*er_dmeasL);

                if(debug_ == 4){
                        std::cout<<" fwd_filter : in left hypothesis dchi2L is "<<dchi2L<<std::endl;
                }
        }

        if ((LR_==0 && lr != -1.0) ||
                        (LR_==1 && lr == 1.0)) {
                double er_dmeasR, dmeasR;
                if(Tof_correc_) {
                        dmeasR = dmeas2[1];
                        er_dmeasR = er_dmeas2[1];
                } else {
                        dmeasR = fabs(HitMdc.dist()[1]);
                        er_dmeasR = HitMdc.erdist()[1];
                }
                dtdc = dmeasR;
                double AR = 1 / ((v_H_T_ea_v_H)[0] + er_dmeasR*er_dmeasR);
                eaNewR.assign(ea - ea_v_H * AR * ea_v_HT);
                double RkR = 1 - (v_H_T_ea_v_H)[0] * AR;

                if(debug_ == 4){
                        std::cout<<" ea_v_H * AR * ea_v_HT is: "<<ea_v_H * AR * ea_v_HT<<std::endl;
                        std::cout<<" v_H is: "<<v_H<<" Ea is: "<<ea<<" v_H_T_ea_v_H is: "<<v_H_T_ea_v_H<<std::endl;
                        std::cout<<" AR is: "<<AR<<" (v_H_T_ea_v_H)[0] * AR is: "<<(v_H_T_ea_v_H)[0]*AR<<std::endl;
                }

                if(0. == RkR) RkR = 1.e-4;
                HepVector diffR = ea_v_H * AR * (dmeasR - estim);
                aNewR = v_a + diffR;
                double sigR = dmeasR -(v_HT * aNewR)[0];
                dtracknew = (v_HT * aNewR)[0];
                dchi2R = (sigR*sigR)/(RkR * er_dmeasR*er_dmeasR);
                if(debug_ == 4){
                        std::cout<<" fwd_filter : in right hypothesis dchi2R is "<<dchi2R<<std::endl;
                }
        }
        // Update Kalman result :
        double dchi2_loc(0);
        if (fabs(dchi2R-dchi2L)<10. && inext>0) {
                KalFitHitMdc & HitMdc_next = HitsMdc_[inext];
                Helix H_fromR(pivot(), aNewR, eaNewR);
                double chi2_fromR(chi2_next(H_fromR, HitMdc_next, csmflag));
                Helix H_fromL(pivot(), aNewL, eaNewL);
                double chi2_fromL(chi2_next(H_fromL, HitMdc_next, csmflag));
#ifdef YDEBUG
                std::cout << "   chi2_fromL = " << chi2_fromL 
                        << ", chi2_fromR = " << chi2_fromR << std::endl;
#endif
                if (fabs(chi2_fromR-chi2_fromL)<10.){
                        int inext2(-1);
                        if (inext+1<HitsMdc_.size())
                                for( int k=inext+1 ; k < HitsMdc_.size(); k++ )
                                        if (!(HitsMdc_[k].chi2()<0)) {
                                                inext2 = k;
                                                break;
                                        }

                        if (inext2 != -1){
                                KalFitHitMdc & HitMdc_next2 = HitsMdc_[inext2];
                                double chi2_fromR2(chi2_next(H_fromR, HitMdc_next2, csmflag));
                                double chi2_fromL2(chi2_next(H_fromL, HitMdc_next2, csmflag));
#ifdef YDEBUG
                                std::cout << "   chi2_fromL2 = " << chi2_fromL2 
                                        << ", chi2_fromR2 = " << chi2_fromR2 << std::endl;
#endif
                                if (fabs(dchi2R+chi2_fromR+chi2_fromR2-(dchi2L+chi2_fromL+chi2_fromL2))<2) {
                                        if (chi2_fromR2<chi2_fromL2)
                                                dchi2L = DBL_MAX;
                                        else 
                                                dchi2R = DBL_MAX;
                                }
                        } 
                }

                if (!(dchi2L == DBL_MAX && dchi2R == DBL_MAX)) {
                        if (dchi2R+chi2_fromR < dchi2L+chi2_fromL){
                                dchi2L = DBL_MAX;
#ifdef YDEBUG
                                std::cout << " choose right..." << std::endl;
#endif
                        } else {
                                dchi2R = DBL_MAX;
#ifdef YDEBUG
                                std::cout << " choose left..." << std::endl;
#endif
                        }
                }
        }
        if ((0 < dchi2R && dchi2R < dchi2cutf_anal[layerid]) ||
                        (0 < dchi2L && dchi2L < dchi2cutf_anal[layerid])) {

                if (((LR_==0 && dchi2R < dchi2L && lr != -1) || 
                                        (LR_==1 && lr == 1)) && 
                                fabs(aNewR[2]-a()[2]) < 1000. && aNewR[2]) {
                        if (nchits_ < (unsigned) nmdc_hit2_ || dchi2R < dchi2cutf_anal[layerid]){
                                nchits_++;
                                if (flag_ster) nster_++;
                                Ea(eaNewR);
                                a(aNewR);
                                chiSq_ += dchi2R;
                                dchi2_loc = dchi2R;
                                if (l_mass_ == lead_){
                                        HitMdc.LR(1);
                                }        
                                update_bit(HitMdc.wire().layer().layerId());
                        }
                } else if (((LR_==0 && dchi2L <= dchi2R && lr != 1) || 
                                        (LR_==1 && lr == -1)) && 
                                fabs(aNewL[2]-a()[2]) < 1000. && aNewL[2]){
                        if (nchits_ < (unsigned) nmdc_hit2_ || dchi2L < dchi2cutf_anal[layerid]){
                                nchits_++;
                                if (flag_ster) nster_++;
                                Ea(eaNewL);
                                a(aNewL);
                                chiSq_ += dchi2L;
                                dchi2_loc = dchi2L;
                                if (l_mass_ == lead_){
                                        HitMdc.LR(-1);
                                }
                                update_bit(HitMdc.wire().layer().layerId());
                        }
                }
        }
        if (dchi2_loc > dchi2_max_) {
                dchi2_max_ = dchi2_loc ;
                r_max_ = pivot().perp();
        }
        dchi2out = dchi2_loc;
        //if(dchi2out == 0) {
        //   dchi2out = ( (dchi2L < dchi2R ) ? dchi2L : dchi2R ) ;
        // }
        return chiSq_;
}


double KalFitTrack::update_hits_csmalign(KalFitHelixSeg& HelixSeg, int inext, Hep3Vector& meas,int way, double& dchi2out, int csmflag ) {


        HepPoint3D ip(0.,0.,0.);

        KalFitHitMdc* HitMdc = HelixSeg.HitMdc();
        double lr = HitMdc->LR();
        int layerid = (*HitMdc).wire().layer().layerId();
        // cout<<"layerid: "<<layerid<<endl;
        double entrangle = HitMdc->rechitptr()->getEntra();

        if(debug_ == 4) {
                std::cout<<"in update_hits(kalFitHelixSeg,...) : the layerid ="<<layerid<<std::endl; 
                std::cout<<" update_hits: lr= " << lr <<std::endl;
        }
        // For taking the propagation delay into account :
        const KalFitWire& Wire = HitMdc->wire();
        int wire_ID = Wire.geoID();

        if (wire_ID<0 || wire_ID>6796){  //bes
                std::cout << " KalFitTrack: wire_ID problem : " << wire_ID 
                        << std::endl;
                return 0;
        } 
        int flag_ster = Wire.stereo();
        double x[3] ={pivot().x(), pivot().y(), pivot().z()};
        double pmom[3] ={momentum().x(), momentum().y(), momentum().z()};
        double tofest(0);
        double phi = fmod(phi0() + M_PI4, M_PI2);
        double csf0 = cos(phi);
        double snf0 = (1. - csf0) * (1. + csf0);
        snf0 = sqrt((snf0 > 0.) ? snf0 : 0.);
        if(phi > M_PI) snf0 = - snf0;
        if (Tof_correc_){
                double t = tanl();
                double dphi(0);

                Helix work = *(Helix*)this;
                work.ignoreErrorMatrix();
                double phi_ip(0);
                Hep3Vector ip(0, 0, 0);
                work.pivot(ip);
                phi_ip = work.phi0();
                if (fabs(phi - phi_ip) > M_PI) {
                        if (phi > phi_ip) phi -= 2 * M_PI;
                        else phi_ip -= 2 * M_PI;
                }
                dphi = phi - phi_ip;

                double l = fabs(radius() * dphi * sqrt(1 + t * t));
                double pmag( sqrt( 1.0 + t*t ) / kappa());
                double mass_over_p( mass_ / pmag );
                double beta( 1.0 / sqrt( 1.0 + mass_over_p * mass_over_p ) );
                tofest = l / ( 29.9792458 * beta );
                if(csmflag==1 && (*HitMdc).wire().y()>0.)  tofest= -1. * tofest;
        }

        const HepSymMatrix& ea = Ea();
        HelixSeg.Ea_pre_fwd(ea);
        HelixSeg.Ea_filt_fwd(ea);


        const HepVector& v_a = a();
        HelixSeg.a_pre_fwd(v_a);
        HelixSeg.a_filt_fwd(v_a);

        /*

           HepPoint3D pivot_work = pivot();
           HepVector a_work = a();
           HepSymMatrix ea_work = Ea();
           KalFitTrack helix_work(pivot_work, a_work, ea_work, 0, 0, 0);
           helix_work.pivot(ip);

           HepVector a_temp = helix_work.a();
           HepSymMatrix ea_temp = helix_work.Ea();

           HelixSeg.Ea_pre_fwd(ea_temp);        
           HelixSeg.a_pre_fwd(a_temp);

        // My God, for the protection purpose 
        HelixSeg.Ea_filt_fwd(ea_temp);
        HelixSeg.a_filt_fwd(a_temp);

         */

        double dchi2R(99999999), dchi2L(99999999);

        HepVector v_H(5, 0);
        v_H[0] =  -csf0 * meas.x() - snf0 * meas.y();
        v_H[3] =  -meas.z();
        HepMatrix v_HT = v_H.T();

        double estim = (v_HT * v_a)[0];
        HepVector ea_v_H = ea * v_H;
        HepMatrix ea_v_HT = (ea_v_H).T();
        HepVector v_H_T_ea_v_H = v_HT * ea_v_H;

        HepSymMatrix eaNewL(5), eaNewR(5);
        HepVector aNewL(5), aNewR(5);
#ifdef YDEBUG
        cout<<"phi  "<<phi<<"  snf0 "<<snf0<<"  csf0  "<<csf0<<endl
                <<"  meas:  "<<meas <<endl;   
        cout<<"the related matrix:"<<endl;
        cout<<"v_H  "<<v_H<<endl;
        cout<<"v_a  "<<v_a<<endl;
        cout<<"ea  "<<ea<<endl;
        cout<<"v_H_T_ea_v_H  "<<v_H_T_ea_v_H<<endl;
        cout<<"LR_= "<< LR_ << "lr= " << lr <<endl;
#endif

        double time = (*HitMdc).tdc();
        //  if (Tof_correc_)
        //  time = time - way*tofest;

        //  double dd = 1.0e-4 * 40.0 * time;
        //the following 3 line : tempory

        double drifttime = getDriftTime(*HitMdc , tofest);
        double ddl = getDriftDist(*HitMdc, drifttime, 0 );
        double ddr = getDriftDist(*HitMdc, drifttime, 1 );
        double erddl = getSigma( *HitMdc, a()[4], 0, ddl);
        double erddr = getSigma( *HitMdc, a()[4], 1, ddr);

        if(debug_ == 4){
                std::cout<<"the pure drift time is "<<drifttime<<std::endl;
                std::cout<<"the drift dist left hypothesis is "<<ddl<<std::endl;
                std::cout<<"the drift dist right hypothesis is "<<ddr<<std::endl;
                std::cout<<"the error sigma left hypothesis is "<<erddl<<std::endl;
                std::cout<<"the error sigma right hypothesis is "<<erddr<<std::endl;
        }
        double dmeas2[2] = { 0.1*ddl , 0.1*ddr }; //millimeter to centimeter
        double er_dmeas2[2] = {0. , 0.} ;

        if(resolflag_ == 1) { 
                er_dmeas2[0] = 0.1*erddl;
                er_dmeas2[1] = 0.1*erddr;
        } 
        else if(resolflag_ == 0) {
                //   int layid = (*HitMdc).wire().layer().layerId();
                //   double sigma = getSigma(layid, dd);
                //   er_dmeas2[0] = er_dmeas2[1] = sigma;
        }

        // Left hypothesis :
        if ((LR_==0 && lr != 1.0) || 
                        (LR_==1 && lr == -1.0)) {

                double er_dmeasL, dmeasL;
                if(Tof_correc_) {
                        dmeasL = (-1.0)*fabs(dmeas2[0]);
                        er_dmeasL = er_dmeas2[0];
                } else {
                        dmeasL = (-1.0)*fabs((*HitMdc).dist()[0]);
                        er_dmeasL = (*HitMdc).erdist()[0];
                }

                double AL = 1 / ((v_H_T_ea_v_H)[0] + er_dmeasL*er_dmeasL);
                eaNewL.assign(ea - ea_v_H * AL * ea_v_HT);
                double RkL = 1 - (v_H_T_ea_v_H)[0] * AL;
                if(0. == RkL) RkL = 1.e-4;

                HepVector diffL = ea_v_H * AL * (dmeasL - estim);

                aNewL = v_a + diffL;
                double sigL = dmeasL -(v_HT * aNewL)[0];
                dchi2L = (sigL * sigL) /  (RkL * er_dmeasL*er_dmeasL);
        }

        if ((LR_==0 && lr != -1.0) ||
                        (LR_==1 && lr == 1.0)) {

                double er_dmeasR, dmeasR;
                if(Tof_correc_) {
                        dmeasR = dmeas2[1];
                        er_dmeasR = er_dmeas2[1];
                } else {
                        dmeasR = fabs((*HitMdc).dist()[1]);
                        er_dmeasR = (*HitMdc).erdist()[1];
                }

                double AR = 1 / ((v_H_T_ea_v_H)[0] + er_dmeasR*er_dmeasR);
                eaNewR.assign(ea - ea_v_H * AR * ea_v_HT);
                double RkR = 1 - (v_H_T_ea_v_H)[0] * AR;
                if(0. == RkR) RkR = 1.e-4;

                HepVector diffR = ea_v_H * AR * (dmeasR - estim);

                aNewR = v_a + diffR;
                double sigR = dmeasR -(v_HT * aNewR)[0];
                dchi2R = (sigR*sigR) /  (RkR * er_dmeasR*er_dmeasR);
        }

        // Update Kalman result :
        double dchi2_loc(0);

#ifdef YDEBUG
        cout<<" track::update_hits......"<<endl;
        std::cout << "   dchi2R = " << dchi2R << ", dchi2L = " << dchi2L 
                << "   estimate =  "<<estim<< std::endl;
        std::cout << "   dmeasR = " << dmeas2[1] 
                << ", dmeasL = " << (-1.0)*fabs(dmeas2[0]) << ", check HitMdc.ddl = " 
                << (*HitMdc).dist()[0] << std::endl;
        std::cout<<"dchi2L : "<<dchi2L<<" ,dchi2R:  "<<dchi2R<<std::endl; 
#endif


        if (fabs(dchi2R-dchi2L)<10. && inext>0) {

                KalFitHitMdc & HitMdc_next = HitsMdc_[inext];

                Helix H_fromR(pivot(), aNewR, eaNewR);
                double chi2_fromR(chi2_next(H_fromR, HitMdc_next,csmflag));
                //double chi2_fromR(chi2_next(H_fromR, HitMdc_next));

                Helix H_fromL(pivot(), aNewL, eaNewL);
                double chi2_fromL(chi2_next(H_fromL, HitMdc_next,csmflag));
                //double chi2_fromL(chi2_next(H_fromL, HitMdc_next));
#ifdef YDEBUG
                std::cout << "   chi2_fromL = " << chi2_fromL 
                        << ", chi2_fromR = " << chi2_fromR << std::endl;
#endif
                if (fabs(chi2_fromR-chi2_fromL)<10.){
                        int inext2(-1);
                        if (inext+1<HitsMdc_.size())
                                for( int k=inext+1 ; k < HitsMdc_.size(); k++ )
                                        if (!(HitsMdc_[k].chi2()<0)) {
                                                inext2 = k;
                                                break;
                                        }

                        if (inext2 != -1){
                                KalFitHitMdc & HitMdc_next2 = HitsMdc_[inext2];
                                //      double chi2_fromR2(chi2_next(H_fromR, HitMdc_next2));
                                //      double chi2_fromL2(chi2_next(H_fromL, HitMdc_next2));
                                double chi2_fromR2(chi2_next(H_fromR, HitMdc_next2, csmflag));
                                double chi2_fromL2(chi2_next(H_fromL, HitMdc_next2, csmflag));
#ifdef YDEBUG
                                std::cout << "   chi2_fromL2 = " << chi2_fromL2 
                                        << ", chi2_fromR2 = " << chi2_fromR2 << std::endl;
#endif
                                if (fabs(dchi2R+chi2_fromR+chi2_fromR2-(dchi2L+chi2_fromL+chi2_fromL2))<2) {
                                        if (chi2_fromR2<chi2_fromL2)
                                                dchi2L = DBL_MAX;
                                        else 
                                                dchi2R = DBL_MAX;
                                }
                        } 
                }

                if (!(dchi2L == DBL_MAX && dchi2R == DBL_MAX)) {
                        if (dchi2R+chi2_fromR < dchi2L+chi2_fromL){
                                dchi2L = DBL_MAX;
#ifdef YDEBUG
                                std::cout << " choose right..." << std::endl;
#endif
                        } else {
                                dchi2R = DBL_MAX;
#ifdef YDEBUG
                                std::cout << " choose left..." << std::endl;
#endif
                        }
                }
        }

        if ((0 < dchi2R && dchi2R < dchi2cutf_calib[layerid]) ||
                        (0 < dchi2L && dchi2L < dchi2cutf_calib[layerid])) {

                if (((LR_==0 && dchi2R < dchi2L && lr != -1) || 
                                        (LR_==1 && lr == 1)) && 
                                fabs(aNewR[2]-a()[2]) < 1000. && aNewR[2]) {
                        if (nchits_ < (unsigned) nmdc_hit2_ || dchi2R < dchi2cutf_calib[layerid]){
                                nchits_++;
                                if (flag_ster) nster_++;
                                if(layerid>19){  
                                        Ea(eaNewR);
                                        HelixSeg.Ea_filt_fwd(eaNewR);
                                        a(aNewR);
                                        HelixSeg.a_filt_fwd(aNewR);
                                }         
                                /*
                                   Ea(eaNewR);
                                   a(aNewR);

                                   KalFitTrack helixR(pivot_work, aNewR, eaNewR, 0, 0, 0);
                                   helixR.pivot(ip);

                                   a_temp = helixR.a();
                                   ea_temp = helixR.Ea();

                                   HelixSeg.Ea_filt_fwd(ea_temp);
                                   HelixSeg.a_filt_fwd(a_temp);
                                //HelixSeg.filt_include(1);

                                 */

                                chiSq_ += dchi2R;
                                dchi2_loc = dchi2R;
                                if (l_mass_ == lead_){
                                        (*HitMdc).LR(1);
                                        HelixSeg.LR(1);
                                }        
                                update_bit((*HitMdc).wire().layer().layerId());
                        }
                } else if (((LR_==0 && dchi2L <= dchi2R && lr != 1) || 
                                        (LR_==1 && lr == -1)) && 
                                fabs(aNewL[2]-a()[2]) < 1000. && aNewL[2]){
                        if (nchits_ < (unsigned) nmdc_hit2_ || dchi2L < dchi2cutf_calib[layerid]){
                                nchits_++;
                                if (flag_ster) nster_++;
                                if(layerid>19){
                                        Ea(eaNewL);
                                        HelixSeg.Ea_filt_fwd(eaNewL);
                                        a(aNewL);
                                        HelixSeg.a_filt_fwd(aNewL);
                                }

                                /* 
                                   Ea(eaNewL);
                                   a(aNewL);

                                   KalFitTrack helixL(pivot_work, aNewL, eaNewL, 0, 0, 0);
                                   helixL.pivot(ip);
                                   a_temp = helixL.a();
                                   ea_temp = helixL.Ea();
                                   HelixSeg.Ea_filt_fwd(ea_temp);
                                   HelixSeg.a_filt_fwd(a_temp);
                                //HelixSeg.filt_include(1);

                                 */

                                chiSq_ += dchi2L;
                                dchi2_loc = dchi2L;
                                if (l_mass_ == lead_){
                                        (*HitMdc).LR(-1);
                                        HelixSeg.LR(-1);
                                }
                                update_bit((*HitMdc).wire().layer().layerId());
                        }
                }
        }

        if (dchi2_loc > dchi2_max_) {
                dchi2_max_ = dchi2_loc ;
                r_max_ = pivot().perp();
        }
        dchi2out = dchi2_loc;
        //  if(dchi2out == 0) {
        //     dchi2out = ( (dchi2L < dchi2R ) ? dchi2L : dchi2R ) ;
        //  }
        return chiSq_;
}


double KalFitTrack::update_hits(KalFitHelixSeg& HelixSeg, int inext, Hep3Vector& meas,int way, double& dchi2out, int csmflag) {


        HepPoint3D ip(0.,0.,0.);

        KalFitHitMdc* HitMdc = HelixSeg.HitMdc();
        double lr = HitMdc->LR();
        int layerid = (*HitMdc).wire().layer().layerId();
        double entrangle = HitMdc->rechitptr()->getEntra();

        if(debug_ == 4) {
                std::cout<<"in update_hits(kalFitHelixSeg,...) : the layerid ="<<layerid<<std::endl; 
                std::cout<<" update_hits: lr= " << lr <<std::endl;
        }
        // For taking the propagation delay into account :
        const KalFitWire& Wire = HitMdc->wire();
        int wire_ID = Wire.geoID();

        if (wire_ID<0 || wire_ID>6796){  //bes
                std::cout << " KalFitTrack: wire_ID problem : " << wire_ID 
                        << std::endl;
                return 0;
        } 
        int flag_ster = Wire.stereo();
        double x[3] ={pivot().x(), pivot().y(), pivot().z()};
        double pmom[3] ={momentum().x(), momentum().y(), momentum().z()};
        double tofest(0);
        double phi = fmod(phi0() + M_PI4, M_PI2);
        double csf0 = cos(phi);
        double snf0 = (1. - csf0) * (1. + csf0);
        snf0 = sqrt((snf0 > 0.) ? snf0 : 0.);
        if(phi > M_PI) snf0 = - snf0;
        if (Tof_correc_){
                double t = tanl();
                double dphi(0);

                Helix work = *(Helix*)this;
                work.ignoreErrorMatrix();
                double phi_ip(0);
                Hep3Vector ip(0, 0, 0);
                work.pivot(ip);
                phi_ip = work.phi0();
                if (fabs(phi - phi_ip) > M_PI) {
                        if (phi > phi_ip) phi -= 2 * M_PI;
                        else phi_ip -= 2 * M_PI;
                }
                dphi = phi - phi_ip;

                double l = fabs(radius() * dphi * sqrt(1 + t * t));
                double pmag( sqrt( 1.0 + t*t ) / kappa());
                double mass_over_p( mass_ / pmag );
                double beta( 1.0 / sqrt( 1.0 + mass_over_p * mass_over_p ) );
                tofest = l / ( 29.9792458 * beta );
                if(csmflag==1 && (*HitMdc).wire().y()>0.)  tofest= -1. * tofest;
        }

        const HepSymMatrix& ea = Ea();
        HelixSeg.Ea_pre_fwd(ea);
        HelixSeg.Ea_filt_fwd(ea);


        const HepVector& v_a = a();
        HelixSeg.a_pre_fwd(v_a);
        HelixSeg.a_filt_fwd(v_a);

        /*

           HepPoint3D pivot_work = pivot();
           HepVector a_work = a();
           HepSymMatrix ea_work = Ea();
           KalFitTrack helix_work(pivot_work, a_work, ea_work, 0, 0, 0);
           helix_work.pivot(ip);

           HepVector a_temp = helix_work.a();
           HepSymMatrix ea_temp = helix_work.Ea();

           HelixSeg.Ea_pre_fwd(ea_temp);        
           HelixSeg.a_pre_fwd(a_temp);

        // My God, for the protection purpose 
        HelixSeg.Ea_filt_fwd(ea_temp);
        HelixSeg.a_filt_fwd(a_temp);

         */


        double dchi2R(99999999), dchi2L(99999999);

        HepVector v_H(5, 0);
        v_H[0] =  -csf0 * meas.x() - snf0 * meas.y();
        v_H[3] =  -meas.z();
        HepMatrix v_HT = v_H.T();

        double estim = (v_HT * v_a)[0];
        HepVector ea_v_H = ea * v_H;
        HepMatrix ea_v_HT = (ea_v_H).T();
        HepVector v_H_T_ea_v_H = v_HT * ea_v_H;

        HepSymMatrix eaNewL(5), eaNewR(5);
        HepVector aNewL(5), aNewR(5);
#ifdef YDEBUG
        cout<<"phi  "<<phi<<"  snf0 "<<snf0<<"  csf0  "<<csf0<<endl
                <<"  meas:  "<<meas <<endl;   
        cout<<"the related matrix:"<<endl;
        cout<<"v_H  "<<v_H<<endl;
        cout<<"v_a  "<<v_a<<endl;
        cout<<"ea  "<<ea<<endl;
        cout<<"v_H_T_ea_v_H  "<<v_H_T_ea_v_H<<endl;
        cout<<"LR_= "<< LR_ << "lr= " << lr <<endl;
#endif

        double time = (*HitMdc).tdc();
        //  if (Tof_correc_)
        //  time = time - way*tofest;

        //  double dd = 1.0e-4 * 40.0 * time;
        //the following 3 line : tempory

        double drifttime = getDriftTime(*HitMdc , tofest);
        double ddl = getDriftDist(*HitMdc, drifttime, 0 );
        double ddr = getDriftDist(*HitMdc, drifttime, 1 );
        double erddl = getSigma( *HitMdc, a()[4], 0, ddl);
        double erddr = getSigma( *HitMdc, a()[4], 1, ddr);

        if(debug_ == 4){
                std::cout<<"the pure drift time is "<<drifttime<<std::endl;
                std::cout<<"the drift dist left hypothesis is "<<ddl<<std::endl;
                std::cout<<"the drift dist right hypothesis is "<<ddr<<std::endl;
                std::cout<<"the error sigma left hypothesis is "<<erddl<<std::endl;
                std::cout<<"the error sigma right hypothesis is "<<erddr<<std::endl;
        }
        double dmeas2[2] = { 0.1*ddl , 0.1*ddr }; //millimeter to centimeter
        double er_dmeas2[2] = {0. , 0.} ;

        if(resolflag_ == 1) { 
                er_dmeas2[0] = 0.1*erddl;
                er_dmeas2[1] = 0.1*erddr;
        } 
        else if(resolflag_ == 0) {
                //   int layid = (*HitMdc).wire().layer().layerId();
                //   double sigma = getSigma(layid, dd);
                //   er_dmeas2[0] = er_dmeas2[1] = sigma;
        }

        // Left hypothesis :
        if ((LR_==0 && lr != 1.0) || 
                        (LR_==1 && lr == -1.0)) {

                double er_dmeasL, dmeasL;
                if(Tof_correc_) {
                        dmeasL = (-1.0)*fabs(dmeas2[0]);
                        er_dmeasL = er_dmeas2[0];
                } else {
                        dmeasL = (-1.0)*fabs((*HitMdc).dist()[0]);
                        er_dmeasL = (*HitMdc).erdist()[0];
                }

                double AL = 1 / ((v_H_T_ea_v_H)[0] + er_dmeasL*er_dmeasL);
                eaNewL.assign(ea - ea_v_H * AL * ea_v_HT);
                double RkL = 1 - (v_H_T_ea_v_H)[0] * AL;
                if(0. == RkL) RkL = 1.e-4;

                HepVector diffL = ea_v_H * AL * (dmeasL - estim);

                aNewL = v_a + diffL;
                double sigL = dmeasL -(v_HT * aNewL)[0];
                dchi2L = (sigL * sigL) /  (RkL * er_dmeasL*er_dmeasL);
        }

        if ((LR_==0 && lr != -1.0) ||
                        (LR_==1 && lr == 1.0)) {

                double er_dmeasR, dmeasR;
                if(Tof_correc_) {
                        dmeasR = dmeas2[1];
                        er_dmeasR = er_dmeas2[1];
                } else {
                        dmeasR = fabs((*HitMdc).dist()[1]);
                        er_dmeasR = (*HitMdc).erdist()[1];
                }

                double AR = 1 / ((v_H_T_ea_v_H)[0] + er_dmeasR*er_dmeasR);
                eaNewR.assign(ea - ea_v_H * AR * ea_v_HT);
                double RkR = 1 - (v_H_T_ea_v_H)[0] * AR;
                if(0. == RkR) RkR = 1.e-4;

                HepVector diffR = ea_v_H * AR * (dmeasR - estim);

                aNewR = v_a + diffR;
                double sigR = dmeasR -(v_HT * aNewR)[0];
                dchi2R = (sigR*sigR) /  (RkR * er_dmeasR*er_dmeasR);
        }

        // Update Kalman result :
        double dchi2_loc(0);

#ifdef YDEBUG
        cout<<" track::update_hits......"<<endl;
        std::cout << "   dchi2R = " << dchi2R << ", dchi2L = " << dchi2L 
                << "   estimate =  "<<estim<< std::endl;
        std::cout << "   dmeasR = " << dmeas2[1] 
                << ", dmeasL = " << (-1.0)*fabs(dmeas2[0]) << ", check HitMdc.ddl = " 
                << (*HitMdc).dist()[0] << std::endl;
#endif

        if (fabs(dchi2R-dchi2L)<10. && inext>0) {

                KalFitHitMdc & HitMdc_next = HitsMdc_[inext];

                Helix H_fromR(pivot(), aNewR, eaNewR);
                double chi2_fromR(chi2_next(H_fromR, HitMdc_next,csmflag));

                Helix H_fromL(pivot(), aNewL, eaNewL);
                double chi2_fromL(chi2_next(H_fromL, HitMdc_next,csmflag));
#ifdef YDEBUG
                std::cout << "   chi2_fromL = " << chi2_fromL 
                        << ", chi2_fromR = " << chi2_fromR << std::endl;
#endif
                if (fabs(chi2_fromR-chi2_fromL)<10.){
                        int inext2(-1);
                        if (inext+1<HitsMdc_.size())
                                for( int k=inext+1 ; k < HitsMdc_.size(); k++ )
                                        if (!(HitsMdc_[k].chi2()<0)) {
                                                inext2 = k;
                                                break;
                                        }

                        if (inext2 != -1){
                                KalFitHitMdc & HitMdc_next2 = HitsMdc_[inext2];
                                double chi2_fromR2(chi2_next(H_fromR, HitMdc_next2, csmflag));
                                double chi2_fromL2(chi2_next(H_fromL, HitMdc_next2, csmflag));
#ifdef YDEBUG
                                std::cout << "   chi2_fromL2 = " << chi2_fromL2 
                                        << ", chi2_fromR2 = " << chi2_fromR2 << std::endl;
#endif
                                if (fabs(dchi2R+chi2_fromR+chi2_fromR2-(dchi2L+chi2_fromL+chi2_fromL2))<2) {
                                        if (chi2_fromR2<chi2_fromL2)
                                                dchi2L = DBL_MAX;
                                        else 
                                                dchi2R = DBL_MAX;
                                }
                        } 
                }

                if (!(dchi2L == DBL_MAX && dchi2R == DBL_MAX)) {
                        if (dchi2R+chi2_fromR < dchi2L+chi2_fromL){
                                dchi2L = DBL_MAX;
#ifdef YDEBUG
                                std::cout << " choose right..." << std::endl;
#endif
                        } else {
                                dchi2R = DBL_MAX;
#ifdef YDEBUG
                                std::cout << " choose left..." << std::endl;
#endif
                        }
                }
        }

        if ((0 < dchi2R && dchi2R < dchi2cutf_calib[layerid]) ||
                        (0 < dchi2L && dchi2L < dchi2cutf_calib[layerid])) {

                if (((LR_==0 && dchi2R < dchi2L && lr != -1) || 
                                        (LR_==1 && lr == 1)) && 
                                fabs(aNewR[2]-a()[2]) < 1000. && aNewR[2]) {
                        if (nchits_ < (unsigned) nmdc_hit2_ || dchi2R < dchi2cutf_calib[layerid]){
                                nchits_++;
                                if (flag_ster) nster_++;

                                Ea(eaNewR);
                                HelixSeg.Ea_filt_fwd(eaNewR);
                                a(aNewR);
                                HelixSeg.a_filt_fwd(aNewR);

                                /*
                                   Ea(eaNewR);
                                   a(aNewR);

                                   KalFitTrack helixR(pivot_work, aNewR, eaNewR, 0, 0, 0);
                                   helixR.pivot(ip);

                                   a_temp = helixR.a();
                                   ea_temp = helixR.Ea();

                                   HelixSeg.Ea_filt_fwd(ea_temp);
                                   HelixSeg.a_filt_fwd(a_temp);
                                //HelixSeg.filt_include(1);

                                 */

                                chiSq_ += dchi2R;
                                dchi2_loc = dchi2R;
                                if (l_mass_ == lead_){
                                        (*HitMdc).LR(1);
                                        HelixSeg.LR(1);
                                }        
                                update_bit((*HitMdc).wire().layer().layerId());
                        }
                } else if (((LR_==0 && dchi2L <= dchi2R && lr != 1) || 
                                        (LR_==1 && lr == -1)) && 
                                fabs(aNewL[2]-a()[2]) < 1000. && aNewL[2]){
                        if (nchits_ < (unsigned) nmdc_hit2_ || dchi2L < dchi2cutf_calib[layerid]){
                                nchits_++;
                                if (flag_ster) nster_++;
                                Ea(eaNewL);
                                HelixSeg.Ea_filt_fwd(eaNewL);
                                a(aNewL);
                                HelixSeg.a_filt_fwd(aNewL);


                                /* 
                                   Ea(eaNewL);
                                   a(aNewL);

                                   KalFitTrack helixL(pivot_work, aNewL, eaNewL, 0, 0, 0);
                                   helixL.pivot(ip);
                                   a_temp = helixL.a();
                                   ea_temp = helixL.Ea();
                                   HelixSeg.Ea_filt_fwd(ea_temp);
                                   HelixSeg.a_filt_fwd(a_temp);
                                //HelixSeg.filt_include(1);

                                 */

                                chiSq_ += dchi2L;
                                dchi2_loc = dchi2L;
                                if (l_mass_ == lead_){
                                        (*HitMdc).LR(-1);
                                        HelixSeg.LR(-1);
                                }
                                update_bit((*HitMdc).wire().layer().layerId());
                        }
                }
        }

        if (dchi2_loc > dchi2_max_) {
                dchi2_max_ = dchi2_loc ;
                r_max_ = pivot().perp();
        }
        dchi2out = dchi2_loc;
        //  if(dchi2out == 0) {
        //     dchi2out = ( (dchi2L < dchi2R ) ? dchi2L : dchi2R ) ;
        //  }
        return chiSq_;
}


double KalFitTrack::chi2_next(Helix& H, KalFitHitMdc & HitMdc ){

        double lr = HitMdc.LR();
        const KalFitWire& Wire = HitMdc.wire();
        int wire_ID = Wire.geoID();
        int layerid = HitMdc.wire().layer().layerId();
        double entrangle = HitMdc.rechitptr()->getEntra();      

        HepPoint3D fwd(Wire.fwd());
        HepPoint3D bck(Wire.bck());
        Hep3Vector wire = (Hep3Vector)fwd -(Hep3Vector)bck;
        Helix work = H;
        work.ignoreErrorMatrix();
        work.pivot((fwd + bck) * .5);
        HepPoint3D x0kal = (work.x(0).z() - bck.z())/ wire.z() * wire + bck;
        H.pivot(x0kal);

        Hep3Vector meas = H.momentum(0).cross(wire).unit();

        if (wire_ID<0 || wire_ID>6796){  //bes
                std::cout << "KalFitTrack : wire_ID problem : " << wire_ID 
                        << std::endl;
                return DBL_MAX;
        } 

        double x[3] ={pivot().x(), pivot().y(), pivot().z()};
        double pmom[3] ={momentum().x(), momentum().y(), momentum().z()};
        double tofest(0);
        double phi = fmod(phi0() + M_PI4, M_PI2);
        double csf0 = cos(phi);
        double snf0 = (1. - csf0) * (1. + csf0);
        snf0 = sqrt((snf0 > 0.) ? snf0 : 0.);
        if(phi > M_PI) snf0 = - snf0;

        if (Tof_correc_) {
                Hep3Vector ip(0, 0, 0);
                Helix work = *(Helix*)this;
                work.ignoreErrorMatrix();
                work.pivot(ip);
                double phi_ip = work.phi0();
                if (fabs(phi - phi_ip) > M_PI) {
                        if (phi > phi_ip) phi -= 2 * M_PI;
                        else phi_ip -= 2 * M_PI;
                }
                double t = tanl();
                double l = fabs(radius() * (phi - phi_ip) * sqrt(1 + t * t));
                double pmag( sqrt( 1.0 + t*t ) / kappa());
                double mass_over_p( mass_ / pmag );
                double beta( 1.0 / sqrt( 1.0 + mass_over_p * mass_over_p ) );
                tofest = l / ( 29.9792458 * beta );
                //  if(csmflag==1 && HitMdc.wire().y()>0.)  tofest= -1. * tofest;
        }

        const HepSymMatrix& ea = H.Ea();
        const HepVector& v_a = H.a();
        double dchi2R(DBL_MAX), dchi2L(DBL_MAX);

        HepVector v_H(5, 0);
        v_H[0] =  -csf0 * meas.x() - snf0 * meas.y();
        v_H[3] =  -meas.z();
        HepMatrix v_HT = v_H.T();

        double estim = (v_HT * v_a)[0];
        HepVector ea_v_H = ea * v_H;
        HepMatrix ea_v_HT = (ea_v_H).T();
        HepVector v_H_T_ea_v_H = v_HT * ea_v_H;

        HepSymMatrix eaNewL(5), eaNewR(5);
        HepVector aNewL(5), aNewR(5);

        //double time = HitMdc.tdc();
        //if (Tof_correc_)
        // time = time - tofest;
        double drifttime = getDriftTime(HitMdc , tofest);
        double ddl = getDriftDist(HitMdc, drifttime , 0 );
        double ddr = getDriftDist(HitMdc, drifttime , 1 );
        double erddl = getSigma( HitMdc, H.a()[4], 0, ddl);
        double erddr = getSigma( HitMdc, H.a()[4], 1, ddr);

        double dmeas2[2] = { 0.1*ddl, 0.1*ddr };
        double er_dmeas2[2] = {0. , 0.};
        if(resolflag_ == 1) { 
                er_dmeas2[0] = 0.1*erddl; 
                er_dmeas2[1] = 0.1*erddr;
        } 
        else if(resolflag_ == 0) {
                //  int layid = HitMdc.wire().layer().layerId();
                //  double sigma = getSigma(layid, dd);
                //  er_dmeas2[0] = er_dmeas2[1] = sigma;
        }

        if ((LR_==0 && lr != 1.0) || 
                        (LR_==1 && lr == -1.0)) {

                double er_dmeasL, dmeasL;
                if(Tof_correc_) {
                        dmeasL = (-1.0)*fabs(dmeas2[0]);
                        er_dmeasL = er_dmeas2[0];
                } else {
                        dmeasL = (-1.0)*fabs(HitMdc.dist()[0]);
                        er_dmeasL = HitMdc.erdist()[0];
                }

                double AL = 1 / ((v_H_T_ea_v_H)[0] + er_dmeasL*er_dmeasL);
                eaNewL.assign(ea - ea_v_H * AL * ea_v_HT);
                double RkL = 1 - (v_H_T_ea_v_H)[0] * AL;
                if(0. == RkL) RkL = 1.e-4;

                HepVector diffL = ea_v_H * AL * (dmeasL - estim);
                aNewL = v_a + diffL;
                double sigL = dmeasL -(v_HT * aNewL)[0];
                dchi2L = (sigL * sigL) /  (RkL * er_dmeasL*er_dmeasL);
        }

        if ((LR_==0 && lr != -1.0) ||
                        (LR_==1 && lr == 1.0)) {

                double er_dmeasR, dmeasR;
                if(Tof_correc_) {
                        dmeasR = dmeas2[1];
                        er_dmeasR = er_dmeas2[1];
                } else {
                        dmeasR = fabs(HitMdc.dist()[1]);
                        er_dmeasR = HitMdc.erdist()[1];
                }

                double AR = 1 / ((v_H_T_ea_v_H)[0] + er_dmeasR*er_dmeasR);
                eaNewR.assign(ea - ea_v_H * AR * ea_v_HT);
                double RkR = 1 - (v_H_T_ea_v_H)[0] * AR;
                if(0. == RkR) RkR = 1.e-4;

                HepVector diffR = ea_v_H * AR * (dmeasR - estim);
                aNewR = v_a + diffR;
                double sigR = dmeasR -(v_HT * aNewR)[0];
                dchi2R = (sigR*sigR) /  (RkR * er_dmeasR*er_dmeasR);
        } 

        if (dchi2R < dchi2L){
                H.a(aNewR); H.Ea(eaNewR);
        } else {
                H.a(aNewL); H.Ea(eaNewL);
        }
        return ((dchi2R < dchi2L) ? dchi2R : dchi2L);
}

double KalFitTrack::chi2_next(Helix& H, KalFitHitMdc & HitMdc, int csmflag){

        double lr = HitMdc.LR();
        const KalFitWire& Wire = HitMdc.wire();
        int wire_ID = Wire.geoID();
        int layerid = HitMdc.wire().layer().layerId();
        double entrangle = HitMdc.rechitptr()->getEntra();      

        HepPoint3D fwd(Wire.fwd());
        HepPoint3D bck(Wire.bck());
        Hep3Vector wire = (Hep3Vector)fwd -(Hep3Vector)bck;
        Helix work = H;
        work.ignoreErrorMatrix();
        work.pivot((fwd + bck) * .5);
        HepPoint3D x0kal = (work.x(0).z() - bck.z())/ wire.z() * wire + bck;
        H.pivot(x0kal);

        Hep3Vector meas = H.momentum(0).cross(wire).unit();

        if (wire_ID<0 || wire_ID>6796){  //bes
                std::cout << "KalFitTrack : wire_ID problem : " << wire_ID 
                        << std::endl;
                return DBL_MAX;
        } 

        double x[3] ={pivot().x(), pivot().y(), pivot().z()};
        double pmom[3] ={momentum().x(), momentum().y(), momentum().z()};
        double tofest(0);
        double phi = fmod(phi0() + M_PI4, M_PI2);
        double csf0 = cos(phi);
        double snf0 = (1. - csf0) * (1. + csf0);
        snf0 = sqrt((snf0 > 0.) ? snf0 : 0.);
        if(phi > M_PI) snf0 = - snf0;

        if (Tof_correc_) {
                Hep3Vector ip(0, 0, 0);
                Helix work = *(Helix*)this;
                work.ignoreErrorMatrix();
                work.pivot(ip);
                double phi_ip = work.phi0();
                if (fabs(phi - phi_ip) > M_PI) {
                        if (phi > phi_ip) phi -= 2 * M_PI;
                        else phi_ip -= 2 * M_PI;
                }
                double t = tanl();
                double l = fabs(radius() * (phi - phi_ip) * sqrt(1 + t * t));
                double pmag( sqrt( 1.0 + t*t ) / kappa());
                double mass_over_p( mass_ / pmag );
                double beta( 1.0 / sqrt( 1.0 + mass_over_p * mass_over_p ) );
                tofest = l / ( 29.9792458 * beta );
                if(csmflag==1 && HitMdc.wire().y()>0.)  tofest= -1. * tofest;
        }

        const HepSymMatrix& ea = H.Ea();
        const HepVector& v_a = H.a();
        double dchi2R(DBL_MAX), dchi2L(DBL_MAX);

        HepVector v_H(5, 0);
        v_H[0] =  -csf0 * meas.x() - snf0 * meas.y();
        v_H[3] =  -meas.z();
        HepMatrix v_HT = v_H.T();

        double estim = (v_HT * v_a)[0];
        HepVector ea_v_H = ea * v_H;
        HepMatrix ea_v_HT = (ea_v_H).T();
        HepVector v_H_T_ea_v_H = v_HT * ea_v_H;

        HepSymMatrix eaNewL(5), eaNewR(5);
        HepVector aNewL(5), aNewR(5);

        //double time = HitMdc.tdc();
        //if (Tof_correc_)
        // time = time - tofest;
        double drifttime = getDriftTime(HitMdc , tofest);
        double ddl = getDriftDist(HitMdc, drifttime , 0 );
        double ddr = getDriftDist(HitMdc, drifttime , 1 );
        double erddl = getSigma( HitMdc, H.a()[4], 0, ddl);
        double erddr = getSigma( HitMdc, H.a()[4], 1, ddr);

        double dmeas2[2] = { 0.1*ddl, 0.1*ddr };
        double er_dmeas2[2] = {0. , 0.};
        if(resolflag_ == 1) { 
                er_dmeas2[0] = 0.1*erddl; 
                er_dmeas2[1] = 0.1*erddr;
        } 
        else if(resolflag_ == 0) {
                //  int layid = HitMdc.wire().layer().layerId();
                //  double sigma = getSigma(layid, dd);
                //  er_dmeas2[0] = er_dmeas2[1] = sigma;
        }

        if ((LR_==0 && lr != 1.0) || 
                        (LR_==1 && lr == -1.0)) {

                double er_dmeasL, dmeasL;
                if(Tof_correc_) {
                        dmeasL = (-1.0)*fabs(dmeas2[0]);
                        er_dmeasL = er_dmeas2[0];
                } else {
                        dmeasL = (-1.0)*fabs(HitMdc.dist()[0]);
                        er_dmeasL = HitMdc.erdist()[0];
                }

                double AL = 1 / ((v_H_T_ea_v_H)[0] + er_dmeasL*er_dmeasL);
                eaNewL.assign(ea - ea_v_H * AL * ea_v_HT);
                double RkL = 1 - (v_H_T_ea_v_H)[0] * AL;
                if(0. == RkL) RkL = 1.e-4;

                HepVector diffL = ea_v_H * AL * (dmeasL - estim);
                aNewL = v_a + diffL;
                double sigL = dmeasL -(v_HT * aNewL)[0];
                dchi2L = (sigL * sigL) /  (RkL * er_dmeasL*er_dmeasL);
        }

        if ((LR_==0 && lr != -1.0) ||
                        (LR_==1 && lr == 1.0)) {

                double er_dmeasR, dmeasR;
                if(Tof_correc_) {
                        dmeasR = dmeas2[1];
                        er_dmeasR = er_dmeas2[1];
                } else {
                        dmeasR = fabs(HitMdc.dist()[1]);
                        er_dmeasR = HitMdc.erdist()[1];
                }

                double AR = 1 / ((v_H_T_ea_v_H)[0] + er_dmeasR*er_dmeasR);
                eaNewR.assign(ea - ea_v_H * AR * ea_v_HT);
                double RkR = 1 - (v_H_T_ea_v_H)[0] * AR;
                if(0. == RkR) RkR = 1.e-4;

                HepVector diffR = ea_v_H * AR * (dmeasR - estim);
                aNewR = v_a + diffR;
                double sigR = dmeasR -(v_HT * aNewR)[0];
                dchi2R = (sigR*sigR) /  (RkR * er_dmeasR*er_dmeasR);
        } 

        if (dchi2R < dchi2L){
                H.a(aNewR); H.Ea(eaNewR);
        } else {
                H.a(aNewL); H.Ea(eaNewL);
        }
        return ((dchi2R < dchi2L) ? dchi2R : dchi2L);
}


double KalFitTrack::getSigma(int layerId, double driftDist ) const {
        double sigma1,sigma2,f;
        driftDist *= 10;//mm
        if(layerId<8){
                if(driftDist<0.5){
                        sigma1=0.112784;      sigma2=0.229274;      f=0.666;
                }else if(driftDist<1.){
                        sigma1=0.103123;      sigma2=0.269797;      f=0.934;
                }else if(driftDist<1.5){
                        sigma1=0.08276;        sigma2=0.17493;      f=0.89;
                }else if(driftDist<2.){
                        sigma1=0.070109;      sigma2=0.149859;      f=0.89;
                }else if(driftDist<2.5){
                        sigma1=0.064453;      sigma2=0.130149;      f=0.886;
                }else if(driftDist<3.){
                        sigma1=0.062383;      sigma2=0.138806;      f=0.942;
                }else if(driftDist<3.5){
                        sigma1=0.061873;      sigma2=0.145696;      f=0.946;
                }else if(driftDist<4.){
                        sigma1=0.061236;      sigma2=0.119584;      f=0.891;
                }else if(driftDist<4.5){
                        sigma1=0.066292;      sigma2=0.148426;      f=0.917;
                }else if(driftDist<5.){
                        sigma1=0.078074;      sigma2=0.188148;      f=0.911;
                }else if(driftDist<5.5){
                        sigma1=0.088657;      sigma2=0.27548;      f=0.838;
                }else{
                        sigma1=0.093089;      sigma2=0.115556;      f=0.367;
                }
        }else{
                if(driftDist<0.5){
                        sigma1=0.112433;      sigma2=0.327548;      f=0.645;
                }else if(driftDist<1.){
                        sigma1=0.096703;      sigma2=0.305206;      f=0.897;
                }else if(driftDist<1.5){
                        sigma1=0.082518;      sigma2=0.248913;      f= 0.934;
                }else if(driftDist<2.){
                        sigma1=0.072501;      sigma2=0.153868;      f= 0.899;
                }else if(driftDist<2.5){
                        sigma1= 0.065535;     sigma2=0.14246;       f=0.914;
                }else if(driftDist<3.){
                        sigma1=0.060497;      sigma2=0.126489;      f=0.918;
                }else if(driftDist<3.5){
                        sigma1=0.057643;      sigma2= 0.112927;     f=0.892;
                }else if(driftDist<4.){
                        sigma1=0.055266;      sigma2=0.094833;      f=0.887;
                }else if(driftDist<4.5){
                        sigma1=0.056263;      sigma2=0.124419;      f= 0.932;
                }else if(driftDist<5.){
                        sigma1=0.056599;      sigma2=0.124248;      f=0.923;
                }else if(driftDist<5.5){
                        sigma1= 0.061377;     sigma2=0.146147;      f=0.964;
                }else if(driftDist<6.){
                        sigma1=0.063978;      sigma2=0.150591;      f=0.942;
                }else if(driftDist<6.5){
                        sigma1=0.072951;      sigma2=0.15685;       f=0.913;
                }else if(driftDist<7.){
                        sigma1=0.085438;      sigma2=0.255109;      f=0.931;
                }else if(driftDist<7.5){
                        sigma1=0.101635;      sigma2=0.315529;      f=0.878;
                }else{
                        sigma1=0.149529;      sigma2=0.374697;      f=0.89;
                }
        }
        double sigmax = sqrt(f*sigma1*sigma1+(1 - f)*sigma2*sigma2)*0.1;
        return sigmax;//cm
}

---