HoughValidUpdate.cxx

Go to the documentation of this file.

#include "MdcHoughFinder/HoughValidUpdate.h" 
#include "GaudiKernel/MsgStream.h"
#include "GaudiKernel/AlgFactory.h"
#include "GaudiKernel/ISvcLocator.h"
#include "GaudiKernel/SmartDataPtr.h"
#include "GaudiKernel/IDataProviderSvc.h"
#include "GaudiKernel/PropertyMgr.h"
#include "GaudiKernel/IService.h"
#include "EventModel/EventHeader.h"
#include "McTruth/DecayMode.h"
#include "McTruth/McParticle.h"
#include "MdcRawEvent/MdcDigi.h"
#include "Identifier/Identifier.h"
#include "Identifier/MdcID.h"
 
 
#include "EvTimeEvent/RecEsTime.h"
#include "RawDataProviderSvc/MdcRawDataProvider.h"
#include "McTruth/MdcMcHit.h" 
#include "MdcRecEvent/RecMdcTrack.h"
#include "MdcRecEvent/RecMdcHit.h"
#include "TrkBase/TrkFit.h"
#include "TrkBase/TrkHitList.h"
#include "TrkBase/TrkExchangePar.h"
#include "TrkFitter/TrkHelixMaker.h"
#include "TrkFitter/TrkCircleMaker.h"
#include "TrkFitter/TrkLineMaker.h"
#include "TrkFitter/TrkHelixFitter.h"
#include "MdcxReco/Mdcxprobab.h"
#include "MdcData/MdcHit.h"
#include "MdcData/MdcRecoHitOnTrack.h"
#include "MdcTrkRecon/MdcTrack.h"
#include "MdcGeom/EntranceAngle.h"
#include "TrackUtil/Helix.h"
#include "GaudiKernel/IPartPropSvc.h"
#include "MdcRecoUtil/Pdt.h"
#include "RawEvent/RawDataUtil.h"
#include "TrkBase/TrkErrCode.h"
#include "MdcPrintSvc/MdcPrintSvc.h"
 
#include "TTree.h"
#include "TF1.h"
#include "TGraph.h"
#include "iomanip"
 
using namespace std;
using namespace Event;
 
/////////////////////////////////////////////////////////////////////////////
 
HoughValidUpdate::HoughValidUpdate(const std::string& name, ISvcLocator* pSvcLocator) :
  Algorithm(name, pSvcLocator)
{
  // Declare the properties  
  declareProperty("drawTuple",      m_drawTuple=0);
  declareProperty("useMcInfo",      m_useMcInfo=0);
  declareProperty("method",         m_method=0);
  declareProperty("debug",          m_debug = 0);
  declareProperty("data",           m_data= 0);
  declareProperty("binx",           m_binx= 100);
  declareProperty("biny",           m_biny= 200);
  declareProperty("maxrho",         m_maxrho= 0.3);
  declareProperty("peakCellNumX",   m_peakCellNumX);
  declareProperty("peakCellNumY",   m_peakCellNumY);
  declareProperty("ndev1",          m_ndev1= 1);
  declareProperty("ndev2",          m_ndev2= 5);
  declareProperty("f_hit_pro",      m_hit_pro= 0.4);
  declareProperty("pdtFile",        m_pdtFile = "pdt.table");
  declareProperty("pickHits",       m_pickHits = true);             // ??
  declareProperty("pid",            m_pid = 0);
  declareProperty("combineTracking",m_combineTracking = false);
  declareProperty("getDigiFlag",    m_getDigiFlag = 0);
  declareProperty("maxMdcDigi",     m_maxMdcDigi = 0);
  declareProperty("keepBadTdc",     m_keepBadTdc = 0);
  declareProperty("dropHot",        m_dropHot= 0);
  declareProperty("keepUnmatch",    m_keepUnmatch = 0);
  declareProperty("minMdcDigi",     m_minMdcDigi = 0);
  declareProperty("setnpeak_fit",   m_setpeak_fit= -1);
  declareProperty("Q",              m_q= 0);
  declareProperty("eventcut",       m_eventcut= -1);
  declareProperty("rhocut",     m_rhocut= -1.);
  declareProperty("mcpar",      m_mcpar= 0);           //if use par truth
  declareProperty("fillTruth",      m_fillTruth= 0);           
  declareProperty("removeBadTrack", m_removeBadTrack= false);           
  declareProperty("dropTrkDrCut",   m_dropTrkDrCut= 1.);           
  declareProperty("dropTrkDzCut",   m_dropTrkDzCut= 10.);           
  declareProperty("dropTrkPtCut",   m_dropTrkPtCut= 99999.);           
  declareProperty("dropTrkChi2Cut", m_dropTrkChi2Cut = 99999.);           
  declareProperty("dropTrkChi2NdfCut", m_dropTrkChi2NdfCut = 99999.);           
  declareProperty("qualityFactor",  m_qualityFactor= 3.);           
  declareProperty("dropTrkNhitCut", m_dropTrkNhitCut= 9);
}
 
StatusCode HoughValidUpdate::beginRun(){
 
  //Initailize MdcDetector
  m_gm = MdcDetector::instance(0);
  if(NULL == m_gm) return StatusCode::FAILURE;
 
  return StatusCode::SUCCESS;
}
 
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * 
StatusCode HoughValidUpdate::initialize(){
 
  MsgStream log(msgSvc(), name());
  log << MSG::INFO << "in initialize()" << endreq;
 
  StatusCode sc;
  //for(int i=0;i<43;i++) TrkHelixFitter::nSigmaCut[i] = m_helixHitsSigma[i];
 
  IPartPropSvc* p_PartPropSvc;
  static const bool CREATEIFNOTTHERE(true);
  sc = service("PartPropSvc", p_PartPropSvc, CREATEIFNOTTHERE);
  if (!sc.isSuccess() || 0 == p_PartPropSvc) {
    log << MSG::ERROR << " Could not initialize PartPropSvc" << endreq;
    return sc;
  }
  m_particleTable = p_PartPropSvc->PDT();
 
  // RawData
  IRawDataProviderSvc* irawDataProviderSvc;
  sc = service ("RawDataProviderSvc", irawDataProviderSvc);
  m_rawDataProviderSvc = dynamic_cast<RawDataProviderSvc*> (irawDataProviderSvc);
  if ( sc.isFailure() ){
    log << MSG::FATAL << name()<<" Could not load RawDataProviderSvc!" << endreq;
    return StatusCode::FAILURE;
 
  }
 
  //  Geometry
  IMdcGeomSvc*   imdcGeomSvc;
  sc = service ("MdcGeomSvc", imdcGeomSvc);
  m_mdcGeomSvc = dynamic_cast<MdcGeomSvc*> (imdcGeomSvc);
  if ( sc.isFailure() ){
    log << MSG::FATAL << "Could not load MdcGeoSvc!" << endreq;
    return StatusCode::FAILURE;
  }
 
  // MdcPrintSvc
  IMdcPrintSvc*  iMdcPrintSvc;
  sc = service ("MdcPrintSvc", iMdcPrintSvc);
  m_mdcPrintSvc= dynamic_cast<MdcPrintSvc*> (iMdcPrintSvc);
  if ( sc.isFailure() ){
    log << MSG::FATAL << "Could not load MdcPrintSvc!" << endreq;
    return StatusCode::FAILURE;
  }
 
  sc = service ("MagneticFieldSvc",m_pIMF);
  if(sc!=StatusCode::SUCCESS) {
    log << MSG::ERROR << "Unable to open Magnetic field service"<<endreq;
  }
  m_bfield = new BField(m_pIMF);
  log << MSG::INFO << "field z = "<<m_bfield->bFieldNominal()<< endreq;
 
  m_context = new TrkContextEv(m_bfield);
 
  Pdt::readMCppTable(m_pdtFile);
 
  //Get MdcCalibFunSvc
  IMdcCalibFunSvc* imdcCalibSvc; 
  sc = service ("MdcCalibFunSvc", imdcCalibSvc);
  m_mdcCalibFunSvc = dynamic_cast<MdcCalibFunSvc*>(imdcCalibSvc);
  if ( sc.isFailure() ){
    log << MSG::FATAL << "Could not load MdcCalibFunSvc!" << endreq;
    return StatusCode::FAILURE;
  }
 
  //initialize ntuplehit
  if(m_drawTuple){
    NTuplePtr nt1(ntupleSvc(), "HoughValidUpdate/hit");
    if ( nt1 ){
      ntuplehit = nt1;
    } else {
      ntuplehit = ntupleSvc()->book("HoughValidUpdate/hit", CLID_ColumnWiseTuple, "hit");
      if(ntuplehit){
    ntuplehit->addItem("eventNum",           m_eventNum);
    ntuplehit->addItem("runNum",             m_runNum);
    ntuplehit->addItem("nHitMc",              m_nHit_Mc,0, 6796);
    ntuplehit->addItem("nHitAxialMc",              m_nHitAxial_Mc);
    ntuplehit->addItem("layerMc",    m_nHit_Mc,   m_layer_Mc);
    ntuplehit->addItem("cellMc",     m_nHit_Mc,   m_cell_Mc);
 
    ntuplehit->addItem("nTrkMC",     m_nTrkMC,0,10);
    ntuplehit->addItem("pidTruth",   m_nTrkMC,m_pidTruth);
    ntuplehit->addItem("costaTruth", m_nTrkMC,m_costaTruth);
    ntuplehit->addItem("ptTruth",    m_nTrkMC,m_ptTruth);
    ntuplehit->addItem("pzTruth",    m_nTrkMC,m_pzTruth);
    ntuplehit->addItem("pTruth",     m_nTrkMC,m_pTruth);
    ntuplehit->addItem("qTruth",     m_nTrkMC,m_qTruth);
    ntuplehit->addItem("drTruth",    m_nTrkMC,m_drTruth);
    ntuplehit->addItem("phiTruth",   m_nTrkMC,m_phi0Truth);
    ntuplehit->addItem("omegaTruth", m_nTrkMC,m_omegaTruth);
    ntuplehit->addItem("dzTruth",    m_nTrkMC,m_dzTruth);
    ntuplehit->addItem("tanlTruth",  m_nTrkMC,m_tanl_mc);
 
    ntuplehit->addItem("nHit",             m_nHit,0, 6796);
    ntuplehit->addItem("nHitDigi",         m_nHitDigi);
    ntuplehit->addItem("nHitAxial",        m_nHitAxial);
    ntuplehit->addItem("nHitStereo",       m_nHitStereo);
    ntuplehit->addItem("layer",         m_nHit,   m_layer);
    ntuplehit->addItem("cell",          m_nHit,   m_cell);
    ntuplehit->addItem("x_east",        m_nHit,   m_x_east);
    ntuplehit->addItem("y_east",        m_nHit,   m_y_east);
    ntuplehit->addItem("z_east",        m_nHit,   m_z_east);
    ntuplehit->addItem("x_west",        m_nHit,   m_x_west);
    ntuplehit->addItem("y_west",        m_nHit,   m_y_west);
    ntuplehit->addItem("z_west",        m_nHit,   m_z_west);
    ntuplehit->addItem("x",             m_nHit,   m_x);
    ntuplehit->addItem("y",             m_nHit,   m_y);
    ntuplehit->addItem("z",             m_nHit,   m_z);
    ntuplehit->addItem("u",             m_nHit,   m_u);
    ntuplehit->addItem("v",             m_nHit,   m_v);
    ntuplehit->addItem("p",             m_nHit,   m_p);
    ntuplehit->addItem("slant",         m_nHit,   m_slant);
    ntuplehit->addItem("layerNhit",        43,    m_layerNhit);
    ntuplehit->addItem("layerMax",                m_layer_max);
    ntuplehit->addItem("l_truth",       m_nHit,   m_ltruth);
    ntuplehit->addItem("z_truth",       m_nHit,   m_ztruth);
    ntuplehit->addItem("z_postruth",    m_nHit,   m_postruth);
    ntuplehit->addItem("digi_truth",    m_nHit,   m_digi_truth);
    ntuplehit->addItem("e_delta",       m_e_delta);
    //hough map
    ntuplehit->addItem("nCross",             m_nCross,     0, 125000);
    ntuplehit->addItem("rho",              m_nCross,     m_rho);
    ntuplehit->addItem("theta",                m_nCross,     m_theta);
    ntuplehit->addItem("xybinNum",           m_xybinNum,   0,100000);
    ntuplehit->addItem("xybin",              m_xybinNum,   m_xybin);
    ntuplehit->addItem("xsigma",             m_xsigma);
    ntuplehit->addItem("ysigma",             m_ysigma);
 
    ntuplehit->addItem("npeak_1",            m_npeak_1);
    ntuplehit->addItem("npeak_2",            m_npeak_2);
    ntuplehit->addItem("trackFailure",       m_trackFailure);
    ntuplehit->addItem("npeak_after_tracking",   m_npeak_after_tracking,       0,1000);
    ntuplehit->addItem("nHitSelect",             m_npeak_after_tracking,       m_nHitSelect);
    ntuplehit->addItem("nHitAxialSelect",      m_npeak_after_tracking,       m_nHitAxialSelect);
    ntuplehit->addItem("nHitStereoSelect",     m_npeak_after_tracking,       m_nHitStereoSelect);
    ntuplehit->addItem("naxiallose",             m_npeak_after_tracking,       m_axiallose);
    ntuplehit->addItem("nstereolose",            m_npeak_after_tracking,       m_stereolose);
 
    ntuplehit->addItem("trackNum_Mc",    m_trackNum_Mc,  0,100);
    ntuplehit->addItem("trackNum",       m_trackNum,     0,100);
    ntuplehit->addItem("trackNum_fit",   m_trackNum_fit,  0,100);
    ntuplehit->addItem("trackNum_tds",   m_trackNum_tds,  0,100);
    ntuplehit->addItem("circleCenterX",  m_trackNum,   m_x_circle);
    ntuplehit->addItem("circleCenterY",  m_trackNum,   m_y_circle);
    ntuplehit->addItem("circleR",        m_trackNum,   m_r);
    ntuplehit->addItem("chis_pt",        m_trackNum,   m_chis_pt);
    ntuplehit->addItem("pt_rec",         m_trackNum,   m_pt);
 
    ntuplehit->addItem("nHitStereo_use",m_nHitStereo_use,0,1000);
    ntuplehit->addItem("l_Calcu_Left",  m_nHitStereo_use,     m_lCalcuLeft);
    ntuplehit->addItem("l_Calcu_Right", m_nHitStereo_use,     m_lCalcuRight);
    ntuplehit->addItem("z_Calcu_Left",  m_nHitStereo_use,     m_zCalcuLeft);
    ntuplehit->addItem("z_Calcu_Right", m_nHitStereo_use,     m_zCalcuRight);
    ntuplehit->addItem("z_Calcu",       m_nHitStereo_use,     m_zCalcu);
    ntuplehit->addItem("l_Calcu",       m_nHitStereo_use,     m_lCalcu);
    ntuplehit->addItem("z_delta",       m_nHitStereo_use,     m_delta_z);
    ntuplehit->addItem("l_delta",       m_nHitStereo_use,     m_delta_l);
    ntuplehit->addItem("amb",           m_nHitStereo_use,     m_amb);
    ntuplehit->addItem("amb_no_match",       m_ambig_no_match);
    ntuplehit->addItem("amb_no_match_pro",   m_ambig_no_match_propotion);
    ntuplehit->addItem("tanl_Cal",           m_tanl_Cal);
    ntuplehit->addItem("z0_Cal",             m_z0_Cal);
 
    ntuplehit->addItem("pt2_rec_final", m_trackNum_fit, m_pt2_rec_final);
    ntuplehit->addItem("p_rec_final",   m_trackNum_fit, m_p_rec_final);
    ntuplehit->addItem("d0_final",      m_trackNum_fit, m_d0_final);
    ntuplehit->addItem("phi0_final",    m_trackNum_fit, m_phi0_final);
    ntuplehit->addItem("omega_final",   m_trackNum_fit, m_omega_final);
    ntuplehit->addItem("z0_final",      m_trackNum_fit, m_z0_final);
    ntuplehit->addItem("tanl_final",    m_trackNum_fit, m_tanl_final);
    ntuplehit->addItem("chis_3d_final", m_trackNum_fit, m_chis_3d_final);
    ntuplehit->addItem("Q_final",       m_trackNum_fit, m_Q);
 
    ntuplehit->addItem("pt2_rec_tds",   m_trackNum_tds, m_pt2_rec_tds);
    ntuplehit->addItem("p_rec_tds",     m_trackNum_tds, m_p_rec_tds);
    ntuplehit->addItem("d0_tds",        m_trackNum_tds, m_d0_tds);
    ntuplehit->addItem("phi0_tds",      m_trackNum_tds, m_phi0_tds);
    ntuplehit->addItem("omega_tds",     m_trackNum_tds, m_omega_tds);
    ntuplehit->addItem("z0_tds",        m_trackNum_tds, m_z0_tds);
    ntuplehit->addItem("tanl_tds",      m_trackNum_tds, m_tanl_tds);
    ntuplehit->addItem("Q_tds",         m_trackNum_tds, m_Q_tds);
 
 
    ntuplehit->addItem("nhitdis0",           m_nhitdis0, 0,2000);
    ntuplehit->addItem("nhitdis1",           m_nhitdis1, 0,2000);
    ntuplehit->addItem("nhitdis2",           m_nhitdis2, 0,2000);
    ntuplehit->addItem("hitdis0",            m_nhitdis0,   m_hit_dis0);
    ntuplehit->addItem("hitdis1",            m_nhitdis1,   m_hit_dis1);
    ntuplehit->addItem("hitdis2",            m_nhitdis2,   m_hit_dis2);
    ntuplehit->addItem("hitdis1_3d",         m_nhitdis1,   m_hit_dis1_3d);
    ntuplehit->addItem("hitdis2_3d",         m_nhitdis2,   m_hit_dis2_3d);
    ntuplehit->addItem("track_use_intrk1",   m_track_use_intrk1);
    ntuplehit->addItem("noise_intrk1",       m_noise_intrk1);
    ntuplehit->addItem("decay",              m_decay);
    ntuplehit->addItem("outputTrk",          m_outputTrk, 0,100);
    ntuplehit->addItem("hitOnTrk",           m_outputTrk,    m_hitOnTrk);
 
 
      } else { log << MSG::ERROR << "Cannot book tuple HoughValidUpdate/hough" << endmsg;
    return StatusCode::FAILURE;
      }
    }
  }
 
  if(m_debug>2) TrkHelixFitter::m_debug = true;
 
  return StatusCode::SUCCESS;
 
 
}
 
 
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * 
StatusCode HoughValidUpdate::execute() {
 
  cout.precision(6);
  MsgStream log(msgSvc(), name());
  log << MSG::INFO << "in execute()" << endreq;
 
  //event start time
  SmartDataPtr<RecEsTimeCol> evTimeCol(eventSvc(),"/Event/Recon/RecEsTimeCol");
  if (!evTimeCol) {
    log << MSG::WARNING<< "Could not retrieve RecEsTimeCol , use t0=0" << endreq;
    m_bunchT0=0.;
  }else{
    RecEsTimeCol::iterator iter_evt = evTimeCol->begin();
    if (iter_evt != evTimeCol->end()){
      m_bunchT0 = (*iter_evt)->getTest()*1.e-9;//m_bunchT0-s, getTest-ns
    }
  }
 
  // Get the event header, print out event and run number
  SmartDataPtr<Event::EventHeader> eventHeader(eventSvc(),"/Event/EventHeader");
  if (!eventHeader) {
    log << MSG::FATAL << "Could not find Event Header" << endreq;
    return( StatusCode::FAILURE);
  }
 
  DataObject *aTrackCol;
  DataObject *aRecHitCol;
  SmartIF<IDataManagerSvc> dataManSvc(eventSvc()); 
  if(!m_combineTracking){
    eventSvc()->findObject("/Event/Recon/RecMdcTrackCol",aTrackCol);
    if(aTrackCol != NULL) {
      dataManSvc->clearSubTree("/Event/Recon/RecMdcTrackCol");
      eventSvc()->unregisterObject("/Event/Recon/RecMdcTrackCol");
    }
    eventSvc()->findObject("/Event/Recon/RecMdcHitCol",aRecHitCol);
    if(aRecHitCol != NULL) {
      dataManSvc->clearSubTree("/Event/Recon/RecMdcHitCol");
      eventSvc()->unregisterObject("/Event/Recon/RecMdcHitCol");
    }
  }
 
  RecMdcTrackCol* trackList;
  eventSvc()->findObject("/Event/Recon/RecMdcTrackCol",aTrackCol);
  if (aTrackCol) {
    trackList = dynamic_cast<RecMdcTrackCol*> (aTrackCol);
  }else{
    trackList = new RecMdcTrackCol;
    StatusCode sc =  eventSvc()->registerObject(EventModel::Recon::RecMdcTrackCol, trackList);
    if(!sc.isSuccess()) {
      log << MSG::FATAL << " Could not register RecMdcTrack collection" <<endreq;
      return StatusCode::FAILURE;
    }
  }
 
  RecMdcHitCol* hitList;
  eventSvc()->findObject("/Event/Recon/RecMdcHitCol",aRecHitCol);
  if (aRecHitCol) {
    hitList = dynamic_cast<RecMdcHitCol*> (aRecHitCol);
  }else{
    hitList = new RecMdcHitCol;
    StatusCode sc =  eventSvc()->registerObject(EventModel::Recon::RecMdcHitCol, hitList);
    if(!sc.isSuccess()) {
      log << MSG::FATAL << " Could not register RecMdcHit collection" <<endreq;
      return StatusCode::FAILURE;
    }
  }
 
  //remove bad quality or low pt track(s)
  if(m_removeBadTrack) {
    vector<RecMdcTrack*> vec_trackList;
    if( m_debug>0 ) cout<<"PATTSF collect "<<trackList->size()<<" track. "<<endl;
    if(trackList->size()!=0){
      RecMdcTrackCol::iterator iter_pat = trackList->begin();
      for(;iter_pat!=trackList->end();iter_pat++){
    double d0=(*iter_pat)->helix(0);
    double kap = (*iter_pat)->helix(2);
    double pt = 0.00001;
    if(fabs(kap)>0.00001) pt = fabs(1./kap);
    double dz=(*iter_pat)->helix(4);
    double chi2=(*iter_pat)->chi2();
    if( m_debug>0) cout<<"d0: "<<d0<<" z0: "<<dz<<" pt "<<pt<<" chi2 "<<chi2<<endl;
    if(fabs(d0)>m_dropTrkDrCut || fabs(dz)>m_dropTrkDzCut || chi2>m_dropTrkChi2Cut || pt>m_dropTrkPtCut) {
      vec_trackList.push_back(*iter_pat);
      //remove hits on track
      HitRefVec rechits = (*iter_pat)->getVecHits();
      HitRefVec::iterator hotIter = rechits.begin();
      while (hotIter!=rechits.end()) {
        if(m_debug>0) cout <<"remove hit " << (*hotIter)->getId() <<endl;
        hitList->remove(*hotIter);
        hotIter++;
      }
      trackList->remove(*iter_pat);
      iter_pat--;
    }
    //  if( m_debug>0 ) cout<<"after delete trackcol size : "<<trackList->size()<<endl;
      }
    } else {
      if(m_debug>0) cout<<" PATTSF find 0 track. "<<endl;
    }
  }//end of remove bad quality high pt track
 
  int nTdsTk=trackList->size();
 
 
  t_eventNum=eventHeader->eventNumber();
  t_runNum=eventHeader->runNumber();
  if( m_drawTuple ){
    m_eventNum=t_eventNum;
    m_runNum=t_runNum;
  }
  if(m_debug>0) cout<<" begin event :"<<t_eventNum<<endl;
  if(t_eventNum<=m_eventcut) {
    cout<<" eventNum cut "<<t_eventNum<<endl; 
    return StatusCode::SUCCESS;
  }
  log << MSG::INFO << "HoughValidUpdate: retrieved event: " << eventHeader->eventNumber()  << "  run: " <<  eventHeader->runNumber() << endreq;
 
  vector<double> vec_u;
  vector<double> vec_v;
  vector<double> vec_p;
  vector<double> vec_x_east;
  vector<double> vec_y_east;
  vector<double> vec_z_east;
  vector<double> vec_x_west;
  vector<double> vec_y_west;
  vector<double> vec_z_west;
  vector<double> vec_z_Mc;
  vector<double> vec_x;
  vector<double> vec_y;
  vector<double> vec_z;
  vector<int> vec_layer;
  vector<int> vec_wire;
  vector<int> vec_slant;
  vector<double> vec_zStereo;
  vector<double> l;
 
  vector<int> vec_layer_Mc;
  vector<int> vec_wire_Mc;
  vector<int> vec_hit_Mc;
  vector<double> vec_ztruth_Mc;
  vector<double> vec_flt_truth_mc;
  vector<double> vec_pos_flag;
  vector<double> vec_phit_MC;
 
  // get mc particle digi
  int mc_particle=GetMcInfo();
  if(m_debug>2) cout<<"MC particle : "<<mc_particle<<endl;
 
  // retrieve Mdc truth hits
  if(m_fillTruth ==1 && m_useMcInfo) {
    int digiId_Mc=0;
    int nHitAxial_Mc=0;
    SmartDataPtr<Event::MdcMcHitCol> mcMdcMcHitCol(eventSvc(),"/Event/MC/MdcMcHitCol"); 
    if (!mcMdcMcHitCol) {
      log << MSG::INFO << "Could not find MdcMcHit" << endreq; 
    }else{
      Event::MdcMcHitCol::iterator iterMcHit = mcMdcMcHitCol->begin();
 
      for(; iterMcHit != mcMdcMcHitCol->end(); ++iterMcHit,digiId_Mc++) {
    Identifier mdcid = (*iterMcHit)->identify();
    int layerId_Mc = MdcID::layer(mdcid);
    int wireId_Mc = MdcID::wire(mdcid);
    double hittemp=layerId_Mc*1000+wireId_Mc;
    double mc_hit_distance =(*iterMcHit)->getDriftDistance();
    double z_mc = (*iterMcHit)->getPositionZ()/10.;//mm 2 cm
    double flt_truth=(z_mc-dz_mc)/tanl_mc;
    double pos_flag=(*iterMcHit)->getPositionFlag();
 
    //    cout<<"flag: "<<(*iterMcHit)->getPositionFlag()<<endl;
    if( (*iterMcHit)->getPositionFlag()==-999 ){
      // cout<<"track Id: "<<(*iterMcHit)->getTrackIndex()<<endl;
      double px= (*iterMcHit)->getPositionX();
      double py= (*iterMcHit)->getPositionY();
      double pz= (*iterMcHit)->getPositionZ();
      double pxyz = sqrt(px*px+py*py+pz*pz);
      vec_phit_MC.push_back(pxyz);
      //      cout<<"PX: "<<px<<endl;
      //      cout<<"PY: "<<py<<endl;
      //      cout<<"PT: "<<sqrt(px*px+py*py)<<endl;
      //      cout<<"PZ: "<<pz<<endl;
      //      cout<<"PXYZ: "<<pxyz<<endl;
      //      cout<<endl;
    }
 
    //  cout<<"( MC:     "  <<layerId_Mc<<","<<wireId_Mc<<"):"<<endl;
    vec_layer_Mc.push_back(layerId_Mc);
    vec_wire_Mc.push_back(wireId_Mc);
    vec_hit_Mc.push_back(hittemp);
    vec_ztruth_Mc.push_back(z_mc);
    vec_flt_truth_mc.push_back(flt_truth);
    vec_pos_flag.push_back(pos_flag);
 
    if(m_debug>5) {
      cout<<"("  <<layerId_Mc<<","<<wireId_Mc<<"):"<<endl;
      cout<<"        hit_distance  :  "<<mc_hit_distance<<endl;
      cout<<"        position flag :  "<<pos_flag<<endl;
      cout<<"        hit_z_mc      :  "<<z_mc<<endl;
      cout<<"        flt_truth     :  "<<flt_truth<<endl;
    }
 
    if(m_data==0){
      if ((layerId_Mc>=8&&layerId_Mc<=19)||(layerId_Mc>=36)) {  
        nHitAxial_Mc++;}
      const MdcGeoWire *geowir =m_mdcGeomSvc->Wire(layerId_Mc,wireId_Mc);
      HepPoint3D eastP = geowir->Backward()/10.0;//mm 2 cm
      HepPoint3D westP = geowir->Forward() /10.0;//mm 2 cm
 
      double x = (*iterMcHit)->getPositionX()/10.;//mm 2 cm
      double y = (*iterMcHit)->getPositionY()/10.;//mm 2 cm
      double z = (*iterMcHit)->getPositionZ()/10.;//mm 2 cm
      double u=CFtrans(x,y);
      double v=CFtrans(y,x);
      double p=sqrt(u*u+v*v);
 
      vec_x_east.push_back(eastP.x());
      vec_y_east.push_back(eastP.y());
      vec_z_east.push_back(eastP.z());
      vec_x_west.push_back(westP.x());
      vec_y_west.push_back(westP.y());
      vec_z_west.push_back(westP.z());
      vec_x.push_back(x);
      vec_y.push_back(y);
      vec_z.push_back(z);
      vec_u.push_back(u);
      vec_v.push_back(v);
      vec_p.push_back(p);
      vec_slant.push_back( SlantId(layerId_Mc) );
 
      m_x_east[digiId_Mc]=eastP.x();
      m_y_east[digiId_Mc]=eastP.y();
      m_z_east[digiId_Mc]=eastP.z();
      m_x_west[digiId_Mc]=westP.x();
      m_y_west[digiId_Mc]=westP.y();
      m_z_west[digiId_Mc]=westP.z();
      m_layer[digiId_Mc]=layerId_Mc;
      m_cell[digiId_Mc]=wireId_Mc;
      m_x[digiId_Mc]=x;
      m_y[digiId_Mc]=y;
      m_z[digiId_Mc]=z;
      m_u[digiId_Mc]=u;
      m_v[digiId_Mc]=v;
      m_p[digiId_Mc]=p;
      m_slant[digiId_Mc]=SlantId(layerId_Mc);
    }
      }
    }
    m_nHit_Mc=digiId_Mc;
    m_nHitAxial_Mc=nHitAxial_Mc;
    if(0==m_data) {m_nHit=digiId_Mc; m_nHitAxial=nHitAxial_Mc;}
  }
 
  //  retrieve Mdc digi vector form RawDataProviderSvc 
  vector<int> vec_hit;
  vector<int> vec_track_index;
  set<int>    hit_noise;            //1
  uint32_t getDigiFlag = 0;
  if(m_dropHot || m_combineTracking)getDigiFlag |= MdcRawDataProvider::b_dropHot;
  if(m_keepBadTdc)  getDigiFlag |= MdcRawDataProvider::b_keepBadTdc;
  if(m_keepUnmatch) getDigiFlag |= MdcRawDataProvider::b_keepUnmatch;
 
  MdcDigiVec mdcDigiVec = m_rawDataProviderSvc->getMdcDigiVec(getDigiFlag);
  if(m_debug>0) cout<<"MdcDigiVec:  "<<mdcDigiVec.size()<<endl;
  MdcDigiVec::iterator iter1 = mdcDigiVec.begin();
  int digiId = 0;
  int nHitAxial = 0;
  for (;iter1 != mdcDigiVec.end(); iter1++, digiId++) {
    Identifier mdcId= (*iter1)->identify();
    int layerId = MdcID::layer(mdcId);
    int wireId = MdcID::wire(mdcId);
    double hittemp=layerId*1000+wireId;
    const  MdcGeoWire *geowir =m_mdcGeomSvc->Wire(layerId,wireId);
    HepPoint3D eastP = geowir->Backward()/10.0;// cm
    HepPoint3D westP = geowir->Forward() /10.0;// cm
    if ((layerId>=8&&layerId<=19)||(layerId>=36))   nHitAxial++;
 
    int track_index=(*iter1)->getTrackIndex();
    if( track_index>=1000 ) track_index-=1000;
    if( track_index<0 )     hit_noise.insert(hittemp); 
    int tchannal=(*iter1)->getTimeChannel();
    int qchannal=(*iter1)->getChargeChannel();
    if( tchannal!=qchannal && m_debug>0 ) cout<<"("<<layerId<<","<<wireId<<")"<< 0<<endl; 
    if( m_debug>3 )   cout<<"track_index in digi: "<<"("<<layerId<<","<<wireId<<"  "<< track_index<<endl;
    if( m_debug>3 )   cout<<"event:  "<<t_eventNum<<"  "<<"layer:  "<<layerId<<" "<<"wireId:  "<<wireId<<" "<<"zeast:  "<<eastP.z()<<"  "<<"zwest:  "<<westP.z()<<"  "<<endl;
 
    //conformal trans:
    double x =(eastP.x()+westP.x())/2.;
    double y =(eastP.y()+westP.y())/2.;
    double u=CFtrans(x,y);
    double v=CFtrans(y,x);
    if(m_debug>3)   cout<<"digiId:  "<<digiId<<" layer:  "<<layerId<<" "<<"wireId:  "<<wireId<<" "<<"x:      "<<x<<"   "<<"y:     "<<y<<"     "<< "u:   "<<u<<"v:   "<<v<<endl;
 
    vec_track_index.push_back(track_index);
    vec_layer.push_back(layerId);
    vec_wire.push_back(wireId);
    vec_hit.push_back(hittemp);
    vec_x_east.push_back(eastP.x());
    vec_y_east.push_back(eastP.y());
    vec_z_east.push_back(eastP.z());
    vec_x_west.push_back(westP.x());
    vec_y_west.push_back(westP.y());
    vec_z_west.push_back(westP.z());
    vec_x.push_back(x);
    vec_y.push_back(y);
    vec_p.push_back(sqrt(u*u+v*v));
    vec_u.push_back(u);
    vec_v.push_back(v);
    vec_slant.push_back( SlantId(layerId) );
 
    if(m_drawTuple){
      m_x_east[digiId]=eastP.x();
      m_y_east[digiId]=eastP.y();
      m_z_east[digiId]=eastP.z();
      m_x_west[digiId]=westP.x();
      m_y_west[digiId]=westP.y();
      m_z_west[digiId]=westP.z();
      m_layer[digiId]=layerId;
      m_cell[digiId]=wireId;
      m_x[digiId]=(vec_x_east[digiId]+vec_x_west[digiId])/2; 
      m_y[digiId]=(vec_y_east[digiId]+vec_y_west[digiId])/2; 
      m_u[digiId]=u;
      m_v[digiId]=v;
      m_p[digiId]=sqrt(u*u+v*v);
      m_slant[digiId]=SlantId( layerId );
      m_layerNhit[layerId]++;   
    }
  }
  if(m_drawTuple){
    m_nHit=digiId;
    m_nHitAxial=nHitAxial;
    m_nHitStereo=m_nHit-m_nHitAxial;
  }
  if(m_debug>0) cout<<"Hit digi number:  "<<digiId<<endl;
  if(digiId<5||nHitAxial<3) {
    if(m_drawTuple) m_trackFailure=1;
    if(m_drawTuple) ntuplehit->write();
    if(m_debug>0) cout<<"not enough hit  "<<endl;
    return StatusCode::SUCCESS;
  }
 
  //correspond mc_fltLenth with digiHit
  vector<int>    vec_digitruth(digiId,0);
  vector<double> vec_flt_truth(digiId,999.);
  vector<double> vec_ztruth(digiId,999.);
  vector<double> vec_posflag_truth(digiId,999.);
  if(m_useMcInfo){
    int if_exit_delta_e=0;
    int nhit_digi=0;
    for(int iHit=0;iHit<digiId;iHit++){
      vector<int>::iterator iter_ihit = find( vec_hit_Mc.begin(),vec_hit_Mc.end(),vec_hit[iHit] );
      if( iter_ihit==vec_hit_Mc.end() ) {
    vec_digitruth[iHit]=0;
    if(m_drawTuple) m_digi_truth[iHit]=0;
    if_exit_delta_e=1;
      }
      else {
    vec_digitruth[iHit]=1;
    m_digi_truth[iHit]=1;
    nhit_digi++;
      }
      for(int iHit_Mc=0;iHit_Mc<vec_flt_truth_mc.size();iHit_Mc++){
    if(vec_hit[iHit]==vec_hit_Mc[iHit_Mc]) {
      vec_flt_truth[iHit]=vec_flt_truth_mc[iHit_Mc];  
      vec_ztruth[iHit]=vec_ztruth_Mc[iHit_Mc];  
      vec_posflag_truth[iHit]=vec_pos_flag[iHit_Mc];
      if(m_drawTuple){
        m_ztruth[iHit]=vec_ztruth_Mc[iHit_Mc];
        m_ltruth[iHit]=vec_flt_truth_mc[iHit_Mc];
        m_postruth[iHit]=vec_pos_flag[iHit_Mc];
      }
    }
      }
      if(m_debug>3) cout<<" hitPos: "<<"("<<vec_layer[iHit]<<","<<vec_wire[iHit]<<")"<<" flt_truth: "<<vec_flt_truth[iHit]<<"   z_truth: "<<vec_ztruth[iHit]<<"   pos_flag: "<<vec_posflag_truth[iHit]<<"   digi_truth: "<<vec_digitruth[iHit]<<endl; 
    }
    if( m_drawTuple ){
      m_e_delta=if_exit_delta_e;
      m_nHitDigi=nhit_digi;
    }
  }
 
  //    calcu the last layer
  vector<int>::iterator laymax=max_element( vec_layer.begin(),vec_layer.end() );
  if(m_drawTuple) m_layer_max=*laymax;  
 
  //-------------------------   finish hit    -------------------------------
  int nhit;          
  int nhitaxial;
  if(m_data==0 && m_useMcInfo && m_fillTruth) {
    nhit=m_nHit_Mc;
    nhitaxial=m_nHitAxial_Mc;
  }
  else{
    nhit=digiId;
    nhitaxial=nHitAxial;
  }
 
  binX=m_binx;
  binY=m_biny;
  TH2D *h1 = new TH2D("h1","h1",binX,0,M_PI,binY,-m_maxrho,m_maxrho);
  TH2D *h2 = new TH2D("h2","h2",binX,0,M_PI,binY,-m_maxrho,m_maxrho);
 
  //method 0
  /* 
     if(m_method==0){
     vector<double> vec_rho;
     vector<double> vec_theta;
     vector< vector<int> > vec_hitNum(2,vector<int>());      //store 2 hits in a cross point
     vector<int> xybin;
  //RhoTheta(nhit,vec_u,vec_v,vec_rho,vec_theta,vec_hitNum,vec_digitruth);    //calculate cross point
  RhoThetaAxial(vec_slant,nhit,vec_u,vec_v,vec_rho,vec_theta,vec_hitNum,vec_digitruth);    //calculate cross point
  FillRhoTheta(h1,vec_theta,vec_rho);         //fill histgram method by rhotheta
  if(m_nCross>125000) return StatusCode::SUCCESS; 
 
  //sum in x
  int xsum_bin;
  int xsum=0;
  TF1 *f =new TF1("f","gaus",0,3.1415926);
  TH1D *hx=new TH1D("hx","hx",binX,0,3.1415926);
  TH1D *hx_new=new TH1D("hx_new","hx_new",m_binx,0,3.1415926);
  for(int ibinx =0;ibinx<binX;ibinx++){
  int t_xsum=0;
  for(int ibiny =0;ibiny<binY;ibiny++){
  t_xsum+=h1->GetBinContent(ibinx+1,ibiny+1);
  }
  hx->SetBinContent(ibinx+1,t_xsum);
  if( xsum <= t_xsum ) {
  xsum=t_xsum;
  xsum_bin=ibinx+1;
  }
  }
  for(int ibinx =0;ibinx<binX;ibinx++){
  int hx_content = hx->GetBinContent(ibinx+1);
  int binx_new=(ibinx+1)-(xsum_bin-(binX/2+1));
  if(binx_new<1)    { binx_new+=binX; }
  if(binx_new>binX) { binx_new-=binX; }
  hx_new->SetBinContent( binx_new , hx_content);
  }
  hx_new->Fit("f","QN");
  double xsigma=f->GetParameter(2);
  delete hx;
  delete hx_new;
 
  //sum in y axial
  TH1D *hy=new TH1D("hy","hy",binY,-0.3,0.3);
  or(int ibiny =0;ibiny<binY;ibiny++){
  int t_ysum=0;
  for(int ibinx =0;ibinx<binX;ibinx++){
  t_ysum+=h1->GetBinContent(ibinx+1,ibiny+1);
  }
  hy->SetBinContent(ibiny+1,t_ysum);
  }
  hy->Fit("f","QN");
  double ysigma=f->GetParameter(2);
  delete hy;
  delete f;
 
  for(int ibinx=0;ibinx<binX;ibinx++){
  for(int ibiny=0;ibiny<binY;ibiny++){
  int binNum=h1->GetBinContent(ibinx+1,ibiny+1);
  xybin.push_back(binNum);
  m_xybin[binY*ibinx+ibiny]=binNum;
  }
  }
  m_xybinNum=binX*binY;
  m_xsigma=xsigma;
  m_ysigma=ysigma;
  }
  */
 
  //method 1
  vector< vector< vector<int> > > vec_selectNum(binX,vector< vector<int> >(binY,vector<int>() ) ); 
  M2 peak_hit_num;
  M1 peak_hit_list;
  int peak_track;
  if(m_method==1){
    FillHist(h1,vec_u,vec_v,nhit,vec_selectNum);
    int max_count=0;
    int max_binx=0;
    int max_biny=0;
    for(int i=0;i<binX;i++){
      for(int j=0;j<binY;j++){
    int count=h1->GetBinContent(i+1,j+1);
    if(max_count<count) {
      max_count=count;
      max_binx=i+1;
      max_biny=j+1;
    }
      }
    }
 
    if(vec_selectNum[max_binx-1][max_biny-1].size() != max_count ) cout<< "ERROR IN VEC!!! "<<endl;
    if(m_debug>4) {
      cout<<"maxBin: ["<<max_binx-1<<","<<max_biny-1<<"]: "<<vec_selectNum[max_binx-1][max_biny-1].size()<<endl;
      for(int i =0;i<vec_selectNum[max_binx-1][max_biny-1].size();i++) cout<<" maxBin select hit: "<<vec_selectNum[max_binx-1][max_biny-1][i]<<endl;
    }
 
    if(m_debug>4) {
      for(int ibinx=0;ibinx<binX;ibinx++){
    for(int ibiny=0;ibiny<binY;ibiny++){
      int bincount=h1->GetBinContent(ibinx+1,ibiny+1);
      if(vec_selectNum[ibinx][ibiny].size() != bincount )  cout<< "ERROR IN VECTORSELECT filling "<<endl;
      if(vec_selectNum[ibinx][ibiny].size() == 0 ) continue;
      cout<<"bin:"<<"["<<ibinx<<","<<ibiny<<"]"<<" select:"<<vec_selectNum[ibinx][ibiny].size()<<endl;
      for(int i=0;i<vec_selectNum[ibinx][ibiny].size();i++){
        int ihit=vec_selectNum[ibinx][ibiny][i];
        cout<<"    hit: "<<ihit<<"  ("<<vec_layer[ihit]<<","<<vec_wire[ihit]<<")"<<endl;
      }
    }
      }
    }
 
    //find peak
    // set threshold
    int count_use=0;
    double sum=0;
    double sum2=0;  
    for(int i=0;i<binX;i++){
      for(int j=0;j<binY;j++){
    int count=h1->GetBinContent(i+1,j+1);
    if(count!=0){
      count_use++;
      sum+=count;
      sum2+=count*count;
    }
      }
    }
    double f_n=m_ndev1;
    double aver=sum/count_use;
    double dev=sqrt(sum2/count_use-aver*aver);
    int min_counts=(int)(aver+f_n*dev);
    if (min_counts<m_ndev2) min_counts=m_ndev2;
    if(m_debug>2) cout<<"aver:  "<<aver<<"  "<<"dev:  "<<dev<<"  min: "<<min_counts<<endl;
 
    vector< vector <int> > hough_trans_CS(binX,vector <int> (binY,0) ); 
    vector< vector <int> > hough_trans_CS_peak(binX,vector <int> (binY,0) ); 
    for(int i=0;i<binX;i++){
      for(int j=0;j<binY;j++){
    hough_trans_CS[i][j]=h1->GetBinContent(i+1,j+1);
      }
    }
 
    int npeak_1=0;
    for (int r=0; r<binY; r++) {
      double binHigh=2*m_maxrho/binY;
      double rho_peak=r*binHigh-m_maxrho;
      for (int a=0; a<binX; a++) {
    long max_around=0;            
    for (int ar=a-1; ar<=a+1; ar++) {
      for (int rx=r-1; rx<=r+1; rx++) {
        int ax;
        if (ar<0) { ax=ar+binX; }
        else if (ar>=binX) { ax=ar-binX; }
        else { ax=ar; }
        if ( (ax!=a || rx!=r) && rx>=0 && rx<binY) {
          if (hough_trans_CS[ax][rx]>max_around) { max_around=hough_trans_CS[ax][rx]; }
        }
      }
    }
 
    if (hough_trans_CS[a][r]<=min_counts||fabs(rho_peak)<m_rhocut) { hough_trans_CS_peak[a][r]=0; }
    else if (hough_trans_CS[a][r]<max_around) { hough_trans_CS_peak[a][r]=0; }
    else if (hough_trans_CS[a][r]==max_around) { hough_trans_CS_peak[a][r]=1; }
    else if (hough_trans_CS[a][r]>max_around) { hough_trans_CS_peak[a][r]=2; }
    if(hough_trans_CS_peak[a][r]!=0) { 
      if(m_debug>2)  cout<<" possible peak1:"<<"["<<a<<","<<r<<"]"<<": "<<hough_trans_CS_peak[a][r]<<"  "<<hough_trans_CS[a][r]<<endl;
      npeak_1++;
    }
      }
    }
 
 
    //find double-point-peaks in parameter space
    int npeak_2=0;
    for (int r=0; r<binY; r++) {
      for (int a=0; a<binX; a++) {
    if (hough_trans_CS_peak[a][r]==1) {
      hough_trans_CS_peak[a][r]=2;
      for (int ar=a-1; ar<=a+1; ar++) {
        for (int rx=r-1; rx<=r+1; rx++) {
          int ax;
          if (ar<0) { ax=ar+binX; }
          else if (ar>=binX) { ax=ar-binX; }
          else { ax=ar; }
          if ( (ax!=a || rx!=r) && rx>=0 && rx<binY) {
        if (hough_trans_CS_peak[ax][rx]==1) { hough_trans_CS_peak[ax][rx]=0; }
          }
        }
      }
    }
    if(hough_trans_CS_peak[a][r]==2) {
      double binHigh=2*m_maxrho/binY;
      double rho_peak=r*binHigh-m_maxrho;
      npeak_2++;
      if(m_debug>2){
        cout<<" possible peak2:  "<<"["<<a<<","<<r<<"]"<<": "<<hough_trans_CS_peak[a][r]<<"  "<<hough_trans_CS[a][r]<<"  rhopeak: "<<rho_peak<<endl;
        for(int i=0;i<hough_trans_CS[a][r];i++){
          int hitNum=vec_selectNum[a][r][i];
          if(m_debug>2) cout<<"         select hit:  "<<hitNum<<"("<<vec_layer[hitNum]<<","<<vec_wire[hitNum]<<")"<<endl;
        }
      }
    }
      }
    }
 
    //fill the histogram again
    for(int i=0;i<binX;i++){
      for(int j=0;j<binY;j++){
    if(hough_trans_CS_peak[i][j]==2){
      h2->SetBinContent(i+1,j+1,hough_trans_CS[i][j]);
    }
      }
    }
 
    vector<int> peakList[3];
    for(int n=0;n<npeak_2;n++){
      for (int r=0; r<binY; r++) {
    for (int a=0; a<binX; a++) {
      if (hough_trans_CS_peak[a][r]==2) {
        peakList[0].push_back(a);
        peakList[1].push_back(r);
        peakList[2].push_back(hough_trans_CS[a][r]);
      }
    }
      }
    }
 
    if(m_drawTuple){
      m_npeak_1=npeak_1;
      m_npeak_2=npeak_2;
    }
    if(m_debug>0) {
      cout<<"npeak_1:    "<<npeak_1<<endl;
      cout<<"npeak_2:    "<<npeak_2<<endl;
    }
 
    //sort peaks by height
    int n_max;
    for (int na=0; na<npeak_2-1; na++) {
      n_max=na;
      for (int nb=na+1; nb<npeak_2; nb++) {
    if (peakList[2][n_max]<peakList[2][nb]) { n_max=nb; }
      }   
      if (n_max!=na) {        // swap
    float swap[3];
    for (int i=0; i<3; i++) {
      swap[i]=peakList[i][na];
      peakList[i][na]=peakList[i][n_max];
      peakList[i][n_max]=swap[i];
    }
      }
    }
 
    if(npeak_2==0||npeak_2>100){
      if(m_debug>0) cout<<" FAILURE IN NPEAK2 !!!!!! "<<endl;
      delete h1;
      delete h2;
      if(m_drawTuple){
    m_trackFailure=2;
    m_npeak_2=-999;
      }
      if(m_drawTuple) ntuplehit->write();
      return StatusCode::SUCCESS;
    }
 
    if(m_debug>2){
      for(int n=0;n<npeak_2;n++){
    int bintheta=peakList[0][n];
    int binrho=peakList[1][n];
    int count=peakList[2][n];
    cout<<"possible peakList after SORT:  "<<n<<": "<<"["<<bintheta<<","<<binrho<<"]: "<<count<<endl;
    for(int i=0;i<count;i++){
      int hitNum=vec_selectNum[bintheta][binrho][i];
      if(m_debug>2) cout<<"                  "<<" select hit:"<<":"<<hitNum<<" ------ "<<"("<<vec_layer[hitNum]<<","<<vec_wire[hitNum]<<")"<<endl;
    }
      }
    }
 
    // combine peak to track
    peak_track=npeak_2;
    Combine(h1,m_peakCellNumX,m_peakCellNumY,vec_selectNum,peak_hit_list,peak_hit_num,peak_track,peakList);
    if(m_drawTuple) m_npeak_after_tracking=peak_track;
 
    if(m_debug>0) cout<<"event"<<t_eventNum<<":  "<<"has found: "<<peak_track<<" track."<<endl;
    for( int i=0;i<peak_track;i++){
      if(m_debug>0) cout<<"peak["<<i<<"] collect "<<peak_hit_num[i]<<" hits "<<endl;
      int nhitaxialselect=0;   // temp for each peak_track
      for(int j =0;j<peak_hit_num[i];j++){
    int hit_number=peak_hit_list[i][j];
    if(vec_slant[hit_number]==0)   nhitaxialselect++ ;
    if(m_debug>0)   cout<<"            "<<" collect hits: ("<<vec_layer[hit_number]<<","<<vec_wire[hit_number]<<")"<<endl;
      }
      if(m_drawTuple){
    m_nHitSelect[i]=peak_hit_num[i];
    m_nHitAxialSelect[i]=nhitaxialselect;
    m_nHitStereoSelect[i]=peak_hit_num[i]-nhitaxialselect;
    m_axiallose[i]=m_nHitAxial-m_nHitAxialSelect[i];
    m_stereolose[i]=m_nHit-m_nHitSelect[i]-m_axiallose[i];
      }
    }
  }
 
  vector<int> vec_hitbeforefit;
  vector<bool> vec_truthHit(nhit,false);
  int peak_fit=peak_track;
  if(m_setpeak_fit!=-1) {
    peak_fit=m_setpeak_fit;
  }
  if(m_drawTuple) m_trackNum=peak_fit;
 
  vector<int> vec_hit_use;
  //  vector<int> vec_hit_use_in2d;
  //  vector<int> vec_hit_use_intrack1;
  //  vector<int> vec_Qchoise;
  vector<TrkRecoTrk*> vec_newTrack;
  vector<TrkRecoTrk*> vec_track_in_tds;
  vector<int> vec_hitOnTrack;
  vector<MdcHit*> vec_for_clean;
  vector<TrkRecoTrk*> vec_trk_for_clean;
  vector<Mytrack> vec_myTrack;
  int track_fit_success=0;
 
  for(track_fit=0;track_fit<peak_fit;track_fit++){
    double d0_before_least=-999.;
    double phi0_before_least=-999.;
    double omega_before_least=-999.;
    map<int,double> hit_to_circle1;
    map<int,double> hit_to_circle2;
    map<int,double> hit_to_circle3;
    if(m_debug>0) cout<<"Do fitting track: "<<track_fit<<endl;
 
    for(int i=0;i<nhit;i++) vec_truthHit[i]=false;
 
    // confirm every track has which hits
    if(m_debug>1) cout<<"candi track["<<track_fit<<"] collect "<<peak_hit_num[track_fit]<<" hits "<<endl;
 
    for(int i=0;i<peak_hit_num[track_fit];i++){
      int hitNum=peak_hit_list[track_fit][i];
      int hittemp=vec_layer[hitNum]*1000+vec_wire[hitNum];
      vector<int>::iterator iter_ihit = find( vec_hit_use.begin(),vec_hit_use.end(),hittemp );
      if( iter_ihit==vec_hit_use.end() )  vec_truthHit[hitNum]=true; 
      if( m_debug >1 && vec_truthHit[hitNum]==true) cout<<"            "<<"collect hit  :"<<"("<<vec_layer[hitNum]<<","<<vec_wire[hitNum]<<")"<<"          "<<endl;;
    }
    if( m_debug >1){
      for(int i=0;i<nhit;i++){
    if(vec_truthHit[i]==false){
      cout<<"candi track "<<"uncollect hit: "<<endl;
      cout<<"             "<<"uncollect hit  :"<<"("<<vec_layer[i]<<","<<vec_wire[i]<<")"<<"            " <<endl;
    }
      }
    }
    // begin chisq fit
    int t_nHitAxialSelect=0;
    for(int i=0;i<nhit;i++){
      if(vec_truthHit[i]==true){
    if(vec_slant[i]==0) t_nHitAxialSelect++;
      }
    }
    if(m_debug>1) cout<<"track "<<track_fit<<" with axial select: "<<t_nHitAxialSelect<<endl;
    if(t_nHitAxialSelect<3 && m_drawTuple){
      m_x_circle[track_fit]=-999.;
      m_y_circle[track_fit]=-999.;
      m_r[track_fit]=-999.;
      m_chis_pt[track_fit] = -999.;
      m_pt[track_fit]=-999.;
      continue;
    }
    double circle_x=0;
    double circle_y=0;
    double circle_r=0;
    int leastSquare=LeastSquare(nhit,vec_x,vec_y,vec_slant,vec_layer,vec_truthHit,d0_before_least,phi0_before_least,omega_before_least,circle_x,circle_y,circle_r);
    //hitdis to circle1
    for(int i=0;i<nhit;i++){
      int hittemp=vec_layer[i]*1000+vec_wire[i];
      double dist_temp=sqrt(pow( (vec_y[i]-circle_y),2)+pow( (vec_x[i]-circle_x),2))-circle_r;
      if(dist_temp>10.) vec_truthHit[i]=false;
      if(m_debug>1&&vec_truthHit[i]==true)  cout<<"IN LEAST1: "<<"("<<vec_layer[i]<<","<<vec_wire[i]<<"):"<<dist_temp<<endl;
    }
 
    t_nHitAxialSelect=0;
    for(int i=0;i<nhit;i++){
      if(vec_truthHit[i]==true){
    if(vec_slant[i]==0) t_nHitAxialSelect++;
      }
    }
    if(m_debug>1) cout<<"track "<<track_fit<<"with axial2 select: "<<t_nHitAxialSelect<<endl;
    if(t_nHitAxialSelect<3) continue;
 
    int leastSquare2=LeastSquare(nhit,vec_x,vec_y,vec_slant,vec_layer,vec_truthHit,d0_before_least,phi0_before_least,omega_before_least,circle_x,circle_y,circle_r);
    for(int i=0;i<nhit;i++){
      int hittemp=vec_layer[i]*1000+vec_wire[i];
      double dist_temp=sqrt(pow( (vec_y[i]-circle_y),2)+pow( (vec_x[i]-circle_x),2))-circle_r;
      hit_to_circle1[hittemp]=dist_temp;
      if(m_debug>1&&vec_truthHit[i]==true)  cout<<" IN LEAST2: "<<"("<<vec_layer[i]<<","<<vec_wire[i]<<"):"<<dist_temp<<endl;
    }
 
    if(m_drawTuple){
      m_x_circle[track_fit]=circle_x;
      m_y_circle[track_fit]=circle_y;
      m_r[track_fit]=circle_r;
      m_chis_pt[track_fit]=circle_r/333.567;
    }
 
    // q select
    TrkHitList* qhits[2];
    const TrkFit* newFit[2]; 
    TrkHitList* qhits2[2];
    const TrkFit* newFit2[2]; 
    TrkRecoTrk* newTrack2[2]; 
 
    int Q_iq[]={-1,1};
    double Q_Chis_2d[]={9999,9999};
    double Q_Chis_3d[]={9999,9999};
    double Q_z[]={999,999};
    double Q_pt2[]={999,999};
    double Q_tanl[]={999,999};
    int Q_ok[2][2]={0};
 
    int q_start=0;
    int q_end=1;
    if(m_q==-1) {q_start=0;q_end=0;}
    if(m_q==1)  {q_start=1;q_end=1;}
    for(int i_q=q_start;i_q<=q_end;i_q++){
      double d0=d0_before_least;
      double phi0=phi0_before_least;
      double omega=omega_before_least;
      double z0=0;
      double tanl=0;
      if(m_debug>0 ){
    cout<<"BY LEASTSQUARE:  "<<endl;
    cout<<"   d0:      "<<d0<<"   d0_mc   "<<d0_mc<<endl;
    cout<<"   phi0:    "<<phi0<<"   phi0_mc   "<<phi0_mc<<endl;
    cout<<"   omega0:  "<<omega<<"   omega_mc   "<<omega_mc<<endl;
      }
 
      vector<double> vec_flt_least;        //use circle info
      for(int i=0;i<nhit;i++){
    double theta_temp;
    if(circle_x==0||vec_x[i]-circle_x==0){
      theta_temp=0;
    }
    else{
      theta_temp=M_PI-atan2(vec_y[i]-circle_y,vec_x[i]-circle_x)+atan2(circle_y,circle_x);
      if(theta_temp>2*M_PI){
        theta_temp=theta_temp-2*M_PI;
      }
      if(theta_temp<0){
        theta_temp=theta_temp+2*M_PI;
      }
    }
    if(Q_iq[i_q]==-1) {
      double l_temp=circle_r*theta_temp;
      vec_flt_least.push_back(l_temp);
    }
    if(Q_iq[i_q]==1) {
      theta_temp=2*M_PI-theta_temp;
      double l_temp=circle_r*(theta_temp);
      vec_flt_least.push_back(l_temp);
    }
      }
      if(m_debug>2) {
    for(int i=0;i<nhit;i++){
      cout<<"By least 2d: "<< "("<<vec_layer[i]<<","<<vec_wire[i]<<"):"<<vec_flt_least[i]<<endl;
    }
      }
 
      //exchange par to 2d
      if(Q_iq[i_q]==-1){
    d0=-d0;
    omega=-1./circle_r;
      }
      if(Q_iq[i_q]==1){
    d0=d0;
    omega=1./circle_r;
    phi0=phi0-M_PI;
      }
 
      // 2d global fit
      double x_cirtemp;
      double y_cirtemp;
      double r_temp;
      TrkExchangePar tt(d0,phi0,omega,z0,tanl);
      TrkCircleMaker circlefactory;
      float chisum =0.;
      TrkRecoTrk* newTrack = circlefactory.makeTrack(tt, chisum, *m_context, m_bunchT0);
      //vec_trk_for_clean.push_back(newTrack);
      bool permitFlips = true;
      bool lPickHits = m_pickHits;
      circlefactory.setFlipAndDrop(*newTrack, permitFlips, lPickHits);
      int nDigi = digiToHots(mdcDigiVec,newTrack,vec_truthHit,getDigiFlag,vec_flt_least,hit_to_circle1,vec_for_clean);
      if(m_debug>2){
    newTrack->printAll(std::cout);
      }
      //fit 
      //  TrkHitList* qhits = newTrack->hits();
      qhits[i_q] = newTrack->hits();
      TrkErrCode err=qhits[i_q]->fit();
      newFit[i_q] = newTrack->fitResult();
 
      //test fit result
      if (!newFit[i_q] || (newFit[i_q]->nActive()<3) || err.failure()!=0) {
    if(m_debug>0){
      cout << "evt "<<t_eventNum<<" global 2d fit failed ";
      if(newFit[i_q]) cout <<" nAct "<<newFit[i_q]->nActive();
      cout<<"ERR1 failure ="<<err.failure()<<endl;
    }
    //  m_2dFit[track_fit]=0;
    Q_ok[i_q][0]=0;
    Q_ok[i_q][1]=0;
    Q_Chis_2d[i_q]=-999.;
    delete newTrack; 
    continue;
      }else{
    Q_ok[i_q][0]=1;
    Q_ok[i_q][1]=0;
    if(m_debug>0) cout <<"evt "<<t_eventNum<< " global 2d fit success"<<endl;
    if(m_debug>2) {
      newTrack->print(std::cout);
    }
    Q_Chis_2d[i_q]=newFit[i_q]->chisq();
 
    TrkExchangePar par = newFit[i_q]->helix(0.);
    d0=par.d0();
    phi0=par.phi0();
    omega=par.omega();
    r_temp=Q_iq[i_q]/par.omega();
    x_cirtemp = sin(par.phi0()) *(par.d0()+1/par.omega()) * -1.; //z axis verse,x_babar = -x_belle
    y_cirtemp = -1.*cos(par.phi0()) *(par.d0()+1/par.omega()) * -1;//???
 
    if(m_debug>1) cout<<" circle after globle 2d:  "<<"("<<x_cirtemp<<","<<y_cirtemp<<","<<r_temp<<")"<<endl;
    if(m_debug>0) cout<<"pt_rec:   "<<-1*Q_iq[i_q]/omega/333.567<<endl;
 
    if(m_drawTuple){
      m_x_circle[track_fit]=x_cirtemp;
      m_y_circle[track_fit]=y_cirtemp;
      m_r[track_fit]=r_temp;
      m_chis_pt[track_fit] = newFit[i_q]->chisq();
      m_pt[track_fit]=r_temp/333.567;
    }
      }
 
      int nfit2d=0;
      if(m_debug>1) cout<<" hot list:"<<endl;
      TrkHotList::hot_iterator hotIter= qhits[i_q]->hotList().begin();
      int lay=((MdcHit*)(hotIter->hit()))->layernumber();
      int wir=((MdcHit*)(hotIter->hit()))->wirenumber();
      int hittemp=lay*1000+wir;
      while (hotIter!=qhits[i_q]->hotList().end()) {
    if(m_debug>1){ cout <<"(" <<((MdcHit*)(hotIter->hit()))->layernumber()
      <<","<<((MdcHit*)(hotIter->hit()))->wirenumber() 
        <<":"<<hotIter->isActive()<<")  ";
    }
    if (hotIter->isActive()==1) nfit2d++;
    hotIter++;
      }
      if(m_debug>0) cout<<"charge "<<i_q<<" In 2Dfit Num Of Active Hits: "<<nfit2d<<endl;
 
      //caculate z position
      if(m_debug>0 && m_drawTuple){
    cout<<" By global 2d charge "<<i_q<<endl;
    cout<<"   d0:  "   <<d0<<"  "<<m_drTruth[0]<<endl;
    cout<<"   phi0:  " <<phi0<<"   "<<m_phi0Truth[0]<<endl;
    cout<<"   omega:  "<<omega<<"  "<<m_omegaTruth[0]<<endl;
    cout<<"   z0:   "  <<z0<<"  "<<m_dzTruth[0]<<endl;
    cout<<"   tanl:   "<<tanl<<"   "<<m_tanl_mc[0]<<endl;
      }
 
      if(m_debug>2) {
    cout<<"least:( "<<circle_x<<","<<circle_y<<","<<circle_r<<endl;
    cout<<"2d:( "<<x_cirtemp<<","<<y_cirtemp<<","<<r_temp<<endl;
      }
      delete newTrack; 
 
      //calcu every hit after 2dfit
      vector<double> vec_flt;
      vector<double> vec_theta_ontrack;
      for(int i=0;i<nhit;i++){
    double theta_temp;
    if(x_cirtemp==0||vec_x[i]-x_cirtemp==0){
      theta_temp=0;
    }
    else{
      theta_temp=M_PI-atan2(vec_y[i]-y_cirtemp,vec_x[i]-x_cirtemp)+atan2(y_cirtemp,x_cirtemp);
      if(theta_temp>2*M_PI){
        theta_temp=theta_temp-2*M_PI;
      }
      if(theta_temp<0){
        theta_temp=theta_temp+2*M_PI;
      }
      if(Q_iq[i_q]==-1) {
        double l_temp=r_temp*theta_temp;
        vec_flt.push_back(l_temp);
        vec_theta_ontrack.push_back(theta_temp);
        //          if(vec_truthHit[i]==true) cout<<"("<<vec_layer[i]<<","<<vec_wire[i]<<"):l:"<<vec_flt[i]<<"  theta:"<<vec_theta_ontrack[i]<<endl;
      }
      if(Q_iq[i_q]==1) {
        theta_temp=2*M_PI-theta_temp;
        double l_temp=r_temp*(theta_temp);
        vec_flt.push_back(l_temp);
        vec_theta_ontrack.push_back(theta_temp);
        //          if(vec_truthHit[i]==true) cout<<"("<<vec_layer[i]<<","<<vec_wire[i]<<"):l:"<<vec_flt[i]<<"  theta:"<<vec_theta_ontrack[i]<<endl;
      }
    }
      }
 
      if(m_debug>3){
    for(int i=0;i<nhit;i++){
      cout<<"i: "<<"theta: "<<vec_theta_ontrack[i]<<" l: "<<vec_flt[i]<<endl;
    }
      }
 
      for(int i=0;i<nhit;i++){
    int hittemp=vec_layer[i]*1000+vec_wire[i];
    double dist_temp=sqrt(pow( (vec_y[i]-y_cirtemp),2)+pow( (vec_x[i]-x_cirtemp),2))-r_temp;
    hit_to_circle2[hittemp]=dist_temp;
      }
 
      vector<double> vec_l_Calcu_Left;
      vector<double> vec_l_Calcu_Right;
      vector<double> vec_z_Calcu_Left;
      vector<double> vec_z_Calcu_Right;
      vector<double> vec_z_Calcu;
      vector<double> vec_l_Calcu;
      vector<double> vec_delta_z;
      vector<double> vec_delta_l;
 
      double l_Calcu_Left=-999.;
      double l_Calcu_Right=-999.;
      double z_Calcu_Left=-999.;
      double z_Calcu_Right=-999.;
      double z_Calcu=-999.;
      double l_Calcu=-999.;
      double delta_z=-999.;
      double delta_l=-999.;
      // z caculate
      MdcDigiVec::iterator iter2 = mdcDigiVec.begin();
      int zPos;
      int digiId = 0;
      for (;iter2 != mdcDigiVec.end(); iter2++, digiId++) {
    if(vec_truthHit[digiId]==false) continue;
    if(vec_slant[digiId]==0)   continue;
    if(vec_theta_ontrack[digiId]>M_PI) continue;
    const MdcDigi* aDigi = *iter2;
    MdcHit *hit = new MdcHit(aDigi, m_gm);
    hit->setCalibSvc(m_mdcCalibFunSvc);
    hit->setCountPropTime(true); 
    zPos=zPosition(*hit,m_bunchT0,x_cirtemp,y_cirtemp,r_temp,digiId,delta_z,delta_l,l_Calcu_Left,l_Calcu_Right,z_Calcu_Left,z_Calcu_Right,z_Calcu,l_Calcu,Q_iq[i_q]);
    if(m_debug>2)  cout<<"in ZS calcu hitPos: "<<"("<<vec_layer[digiId]<<","<<vec_wire[digiId]<<")"<<"  l_truth: "<<vec_flt_truth[digiId]<<"  z_truth: "<<vec_ztruth[digiId]<<" l_Cal: "<<l_Calcu<<" z_Cal: "<<z_Calcu<<endl;
    delete hit;
    if(zPos==-1||zPos==-2) continue;
    vec_l_Calcu_Left.push_back(l_Calcu_Left);
    vec_l_Calcu_Right.push_back(l_Calcu_Right);
    vec_z_Calcu_Left.push_back(z_Calcu_Left);
    vec_z_Calcu_Right.push_back(z_Calcu_Right);
    vec_z_Calcu.push_back(z_Calcu);
    vec_l_Calcu.push_back(l_Calcu_Right);
    vec_delta_z.push_back(delta_z);
    vec_delta_l.push_back(delta_l);
      }
      int nHitUseInZs=vec_delta_z.size();
 
      if(m_drawTuple){ 
    for(int i=0;i<nHitUseInZs;i++){
      m_nHitStereo_use=vec_delta_z.size();
      m_lCalcuLeft[i]=vec_l_Calcu_Left[i];
      m_lCalcuRight[i]=vec_l_Calcu_Right[i];
      m_zCalcuLeft[i]=vec_z_Calcu_Left[i];
      m_zCalcuRight[i]=vec_z_Calcu_Right[i];
      m_lCalcu[i]=vec_l_Calcu[i];
      m_zCalcu[i]=vec_z_Calcu[i];
      m_delta_z[i]=vec_delta_z[i];
      m_delta_l[i]=vec_delta_l[i];
    }
      }
 
      //ambig choose 
      vector< vector< vector <double> > >  point(2, vector< vector<double> >(2,vector<double>() ));
      for(int iHit=0;iHit<nHitUseInZs;iHit++){
    point[0][0].push_back(vec_l_Calcu_Left[iHit]);
    point[0][1].push_back(vec_z_Calcu_Left[iHit]);
    point[1][0].push_back(vec_l_Calcu_Right[iHit]);
    point[1][1].push_back(vec_z_Calcu_Right[iHit]);
      }
      vector<int> ambigList;
      if(nHitUseInZs!=0){
    AmbigChoose(point,nHitUseInZs,ambigList,tanl,z0);
    if(m_drawTuple){
      m_tanl_Cal=tanl;
      m_z0_Cal=z0;
    }
 
    if(m_useMcInfo && m_drawTuple){
      int ambig_no_match_num=0;
      for(int iHit=0;iHit<nHitUseInZs;iHit++){
        if(ambigList[iHit]==0) ambigList[iHit]=1;
        else ambigList[iHit]=0;
        if(ambigList[iHit]!=m_postruth[iHit]) { m_ambig_no_match=1; ambig_no_match_num++; }
      }
      m_ambig_no_match_propotion=(double)(ambig_no_match_num)/m_nHitStereo_use;
      for (int iHit=0;iHit<ambigList.size();iHit++){
        m_amb[iHit]=ambigList[iHit];
      }
    }
      }
 
      if(m_debug>0 && m_drawTuple){
    cout<<"By 3d track zs fit:"<<endl;
    cout<<"d0: "<<d0<<"  mc : "<<m_drTruth[0]<<endl;
    cout<<"phi0: "<<phi0<<"  mc : "<<m_phi0Truth[0]<<endl;
    cout<<"omega: "<<omega<<"  mc : "<<m_omegaTruth[0]<<endl;
    cout<<"z0: "<<z0<<"  mc : "<<m_dzTruth[0]<<endl;
    cout<<"tanl: "<<tanl<<"  mc : "<<m_tanl_mc[0]<<endl;
      }
 
      if(m_mcpar==1){
    d0=m_drTruth[0];
    phi0=m_phi0Truth[0];
    if(Q_iq[i_q]==-1) {phi0=m_phi0Truth[0]-3./2.*M_PI;omega=m_omegaTruth[0];}
    else                {phi0=m_phi0Truth[0]-1./2.*M_PI;omega=-1*m_omegaTruth[0];}
    z0=m_dzTruth[0];
    tanl=m_tanl_mc[0];
      }
 
      //-------------------------------------------5 parameter fit-----------------------
 
      if(m_debug>0) cout<< "become 3d fit   "<<endl;
      TrkExchangePar tt2(d0,phi0,omega,z0,tanl);
      TrkHelixMaker helixfactory;
      chisum =0.;
      newTrack2[i_q] = helixfactory.makeTrack(tt2, chisum, *m_context, m_bunchT0);
      vec_trk_for_clean.push_back(newTrack2[i_q]);
      permitFlips = true;
      lPickHits = m_pickHits;
      helixfactory.setFlipAndDrop(*newTrack2[i_q], permitFlips, lPickHits);
      int nDigi2 = digiToHots2(mdcDigiVec,newTrack2[i_q],vec_truthHit,vec_flt_truth,getDigiFlag,vec_slant,vec_l_Calcu,vec_flt,vec_theta_ontrack,vec_hitbeforefit,hit_to_circle2,vec_for_clean,tanl);
      if(m_debug>2){
    cout<<__FILE__<<__LINE__<<"AFTER digiToHots 3d ---------BEFORE 3d fit"<<endl;
    newTrack2[i_q]->printAll(std::cout);
      }
      //fit
      qhits2[i_q] = newTrack2[i_q]->hits();
      TrkErrCode err2=qhits2[i_q]->fit();
      newFit2[i_q] = newTrack2[i_q]->fitResult();
      int nActiveHit = newTrack2[i_q]->hots()->nActive();
      int fitSuccess_3d = 0;
      if (!newFit2[i_q] || (newFit2[i_q]->nActive()<5) || err2.failure()!=0) {
    fitSuccess_3d=0;
    Q_ok[i_q][1]=0;
    Q_Chis_3d[i_q]=-999.;
    if(m_debug>0){
      cout << "evt "<<t_eventNum<<" global 3d fit failed ";
      if(!newFit2[i_q]) cout<<" newfit2 point is NULL!!!" <<endl;
      if(newFit2[i_q]) cout <<" nAct "<<newFit2[i_q]->nActive();
      cout<<"ERR2 failure ="<<err2.failure()<<endl;
    }
      }else{
    //fit success
    TrkExchangePar par2 = newFit2[i_q]->helix(0.);
    if( abs( 1/(par2.omega()) )>0.001){
      fitSuccess_3d=1;
      Q_Chis_3d[i_q]=newFit2[i_q]->chisq();
      Q_ok[i_q][1]=1;
      if(m_debug>0) cout <<"evt "<<t_eventNum<< " global 3d fit success "<<err2.failure()<<endl;
      if(m_debug>2){
        cout<<__FILE__<<__LINE__<<"AFTER fit 3d "<<endl;
        newTrack2[i_q]->print(std::cout);
      }
    } else {
      fitSuccess_3d=0;
      Q_ok[i_q][1]=0;
      Q_Chis_3d[i_q]=-999.;
      if(m_debug>2) cout<<"3dfit failure of omega "<<endl;
    }
    bool badQuality = false;
    //yzhang add 2016-06-15 
    //test quality of track
    if(fabs(Q_Chis_3d[i_q])>m_qualityFactor*m_dropTrkChi2Cut ){
      if(m_debug>0){
        std::cout<<__FILE__<<"   "<<__LINE__<<" drop track by chi2  "<<Q_Chis_3d[i_q]<<" > "<<m_qualityFactor<<" * "<<m_dropTrkChi2Cut <<std::endl;
      }
      badQuality=1;
    }
    if( fabs(par2.d0())>m_qualityFactor*m_dropTrkDrCut) {
      if(m_debug>0){
        std::cout<<__FILE__<<"   "<<__LINE__<<" drop track by d0 "<<par2.d0()<<" > "<<m_qualityFactor<<" * "<<m_dropTrkDrCut <<std::endl;
      }
      badQuality=1;
    }
    if( fabs(par2.z0())>m_qualityFactor*m_dropTrkDzCut) {
      if(m_debug>0){
        std::cout<<__FILE__<<"   "<<__LINE__<<" drop track by z0 "<<par2.z0()<<" > "<<m_qualityFactor<<" * "<<m_dropTrkDzCut <<std::endl;
      }
      badQuality=1;
    }
    if( (fabs(Q_Chis_3d[i_q])/qhits2[i_q]->nHit()) > m_qualityFactor*m_dropTrkChi2NdfCut) {
      if(m_debug>0){
        std::cout<<__FILE__<<"   "<<__LINE__<<" drop track by chi2/ndf"<<(fabs(Q_Chis_3d[i_q])/qhits2[i_q]->nHit()) <<" > "<<m_qualityFactor<<" * "<<m_dropTrkChi2NdfCut<<std::endl;
      }
      badQuality=1;
    }
    if( nActiveHit <= m_dropTrkNhitCut) {
      if(m_debug>0){
        std::cout<<__FILE__<<"   "<<__LINE__<<" drop track by nhit"<<nActiveHit <<" <= "<<m_qualityFactor<<" * "<<m_dropTrkNhitCut<<std::endl;
      }
      badQuality=1;
    }
    if(badQuality) {
      fitSuccess_3d=0;
      Q_ok[i_q][1]=0;
      Q_Chis_3d[i_q]=-999.;
      if(m_debug>2) cout<<"3dfit failure of bad quality"<<endl;
    }
    //zhangy
      }
 
      // -------------try to out put parameters---
      if(fitSuccess_3d==1){
    TrkExchangePar par2 = newFit2[i_q]->helix(0.);
    if(m_debug>0){
      cout<<"BY 3d global fit charge  "<<i_q<<endl;
      cout<<"d0:                  "<<par2.d0()<<endl;
      cout<<"phi0:                 "<<par2.phi0()<<endl;
      cout<<"w:               "<<par2.omega()<<endl;
      cout<<"z:               "<<par2.z0()<<endl;
      cout<<"tanl:               "<<par2.tanDip()<<endl;
    }
    r_temp=Q_iq[i_q]/par2.omega();
    x_cirtemp = sin(par2.phi0()) *(par2.d0()+1/par2.omega()) * -1.; //z axis verse,x_babar = -x_belle
    y_cirtemp = -1.*cos(par2.phi0()) *(par2.d0()+1/par2.omega()) * -1;//???
    if(m_debug>0) cout<<" circle after globle 3d:  "<<"("<<x_cirtemp<<","<<y_cirtemp<<","<<r_temp<<")"<<endl;
    double pxy=-1*r_temp/333.567;
    double pz=pxy*par2.tanDip();
    double pxyz=sqrt(pxy*pxy+pz*pz);
 
    Q_pt2[i_q]=fabs(pxy);
    Q_z[i_q]=par2.z0();
    Q_tanl[i_q]=par2.tanDip();
      } else{
    continue;
      }
      //------------------------------------clean-------------------------
 
      for(int i=0;i<nhit;i++){
    int hittemp=vec_layer[i]*1000+vec_wire[i];
    double dist_temp=sqrt(pow( (vec_y[i]-y_cirtemp),2)+pow( (vec_x[i]-x_cirtemp),2))-r_temp;
    hit_to_circle3[hittemp]=dist_temp;
      }
    }
 
    if(m_debug>3){
      cout<<"chis 2d: "<<Q_Chis_2d[0]<<"  "<<Q_Chis_2d[1]<<endl;
      cout<<"chis 3d: "<<Q_Chis_3d[0]<<"  "<<Q_Chis_3d[1]<<endl;
      cout<<"Z: "<<Q_z[0]<<"  "<<Q_z[1]<<endl;
      cout<<"chis ok-: "<<Q_ok[0][0]<<"  "<<Q_ok[0][1]<<endl;
      cout<<"chis ok+: "<<Q_ok[1][0]<<"  "<<Q_ok[1][1]<<endl;
    }
 
    int Q;
    int Q_judge[2]={0};
    if(Q_ok[0][1]==1)  Q_judge[0]=1;
    if(Q_ok[1][1]==1)  Q_judge[1]=1;
    if(Q_judge[0]==1&&Q_judge[1]==0) Q=-1;
    if(Q_judge[0]==0&&Q_judge[1]==1) Q=1;
    if(Q_judge[0]==1&&Q_judge[1]==1) {
      if(fabs(Q_z[0])>fabs(Q_z[1])) Q=1;
      else Q=-1;
    }
    if(Q_judge[0]==0&&Q_judge[1]==0) {Q=0; continue;}
    int q_choose=0;
    if(Q==-1) q_choose=0;
    if(Q== 1) q_choose=1;
    TrkExchangePar  par_final = newFit2[q_choose]->helix(0.);
    if(m_debug>0){
      cout<<"after q choose By 3d global fit:  "<<Q<<endl;
      cout<<"d0:                  "<<par_final.d0()<<endl;
      cout<<"phi0:                 "<<par_final.phi0()<<endl;
      cout<<"w:               "<<par_final.omega()<<endl;
      cout<<"z:               "<<par_final.z0()<<endl;
      cout<<"tanl:               "<<par_final.tanDip()<<endl;
    }
    double pxy=-1*Q_iq[q_choose]/par_final.omega()/333.567;
    double pz=pxy*par_final.tanDip();
    double pxyz=sqrt(pxy*pxy+pz*pz);
 
    if(m_debug>0)  cout<<"pt2_rec:   "<<pxy<<endl;
    if(m_drawTuple){
      m_pt2_rec_final[track_fit_success]=pxy;
      m_p_rec_final[track_fit_success]=pxyz;
      m_d0_final[track_fit_success]=par_final.d0();
      m_phi0_final[track_fit_success]=par_final.phi0();
      m_omega_final[track_fit_success]=par_final.omega();
      m_z0_final[track_fit_success]=par_final.z0();
      m_tanl_final[track_fit_success]=par_final.tanDip();
      m_Q[track_fit_success]=Q;
    }
 
    int nfit3d=0;
    if(m_debug>1) cout<<" hot list:"<<endl;
    TrkHotList::hot_iterator hotIter2= qhits2[q_choose]->hotList().begin();
    while (hotIter2!=qhits2[q_choose]->hotList().end()) {
      int lay=((MdcHit*)(hotIter2->hit()))->layernumber();
      int wir=((MdcHit*)(hotIter2->hit()))->wirenumber();
      int hittemp=lay*1000+wir;
      if(m_debug>1){ cout <<"(" <<((MdcHit*)(hotIter2->hit()))->layernumber()
    <<","<<((MdcHit*)(hotIter2->hit()))->wirenumber() 
      <<":"<<hotIter2->isActive()<<")  ";
      }
      if( hotIter2->isActive()==1) {
    nfit3d++; 
    if(track_fit_success==0) vec_hitOnTrack.push_back(hittemp);  //single track
      }
      hotIter2++;
    }
    if(m_debug>0) cout<<"In 3D fit Num Of Active Hits: "<<nfit3d<<endl;
 
    track_fit_success++;
    int choise_temp,choise_no;
    if(Q==-1) choise_temp=0,choise_no=1; 
    if(Q==1)  choise_temp=1,choise_no=0; 
    vec_newTrack.push_back( newTrack2[choise_temp] );
  }       //end loop of all track
 
  for(int iTrack=0;iTrack<vec_newTrack.size();iTrack++){
    const TrkFit* newFit_combine = vec_newTrack[iTrack]->fitResult();
    TrkExchangePar  par_combine= newFit_combine->helix(0.);
    double cx_combine=(-333.567/par_combine.omega()+par_combine.d0())*cos(par_combine.phi0());                   
    double cy_combine=(-333.567/par_combine.omega()+par_combine.d0())*sin(par_combine.phi0());                   
    double phi_combine=par_combine.phi0()-M_PI/2;
    double pxy_combine=1./par_combine.omega()/333.567;
    if(pxy_combine<0)  phi_combine+=M_PI;
    if(phi_combine<0)  phi_combine+=2*M_PI;
    if(phi_combine>2*M_PI)  phi_combine-=2*M_PI;
 
    Mytrack mytrack;
    mytrack.pt=pxy_combine;
    mytrack.charge=(int)(fabs(pxy_combine)/pxy_combine);
    mytrack.phi=phi_combine;
    mytrack.r=-333.567/par_combine.omega();
    mytrack.x_cir=cx_combine;
    mytrack.y_cir=cy_combine;
    mytrack.newTrack=vec_newTrack[iTrack];
    mytrack.use_in_tds=true;
    mytrack.vec_hit_on_trk=vec_hitOnTrack;
    //  cout<<"size of hitontrk: "<<mytrack.vec_hit_on_trk.size()<<endl;
    vec_myTrack.push_back(mytrack);
 
  }
 
  std::sort(vec_myTrack.begin(),vec_myTrack.end(),compare);
 
  for(int i=0;i<vec_newTrack.size();i++){
    Mytrack *mytrack_i=&(vec_myTrack[i]);
    if(mytrack_i->use_in_tds==false) continue;
    for(int j=i+1;j<vec_newTrack.size();j++){
      Mytrack *mytrack_j=&(vec_myTrack[j]);
      if(mytrack_j->use_in_tds==false) continue;
      if( fabs(mytrack_j->phi-mytrack_i->phi)<0.1 ) mytrack_j->use_in_tds=false; 
      if(fabs(mytrack_j->r)*(1.-0.25) <= fabs(mytrack_i->r) && fabs(mytrack_i->r) <= fabs(mytrack_j->r)*(1.+0.25)){
    if(fabs(mytrack_j->x_cir-mytrack_i->x_cir) <= fabs(mytrack_j->r)*(0.25)&& fabs(mytrack_j->y_cir-mytrack_i->y_cir) <= fabs(mytrack_j->r)*(0.25) ){  
      if(mytrack_j->charge==mytrack_i->charge)
        mytrack_j->use_in_tds=false;
    }
      }
    }
  }
 
  int nTrack_tds=0;
  vector<Mytrack> vec_mytrack_in_tds;
  for(int i=0;i<track_fit_success;i++){
    Mytrack mytrack=vec_myTrack[i];
    if(mytrack.use_in_tds==false) continue;
    vec_mytrack_in_tds.push_back(mytrack);
    nTrack_tds++;
  }
 
  if(m_drawTuple){
    m_trackNum_tds=nTrack_tds;
    m_trackNum_fit=track_fit_success;
  }
 
  //  RecMdcTrackCol::iterator iteritrk = trackList->begin();
  //  for(;iteritrk!=trackList->end();iteritrk++){
  //    cout<<"IN PATTSF: "<<endl;
  //    cout<<"parameter: "<<(*iteritrk)->helix(0)<<"  "<<(*iteritrk)->helix(1)<<"  "<<(*iteritrk)->helix(2)<<"  "<<(*iteritrk)->helix(3)<<"  "<<(*iteritrk)->helix(4)<<endl;
  //    cout<<"chi: "<<(*iteritrk)->chi2()<<" ndof: "<<(*iteritrk)->ndof()<<"nster: "<<(*iteritrk)->nster()<<endl;
  //  }
 
  //print track in pattsf with bad vertex
  //    RecMdcTrackCol::iterator iteritrk_pattsf = vec_trackList.begin();
  //    for(;iteritrk_pattsf!=vec_trackList.end();iteritrk_pattsf++){
  //      cout<<"in PATTSF LOST: "<<(*iteritrk_pattsf)->helix(0)<<" "<<(*iteritrk_pattsf)->helix(1)<<" "<<(*iteritrk_pattsf)->helix(2)<<" "<<(*iteritrk_pattsf)->helix(3)<<" "<<(*iteritrk_pattsf)->helix(4)<<endl;
  //    }
 
  int outputTrk=0;
  int itrack=0;
  for(int i=0;i<nTrack_tds;i++){
    Mytrack mytrack=vec_mytrack_in_tds[i];
    const TrkFit* newFit_tds= mytrack.newTrack->fitResult();
    TrkExchangePar  par_tds= newFit_tds->helix(0.);
    //  cout<<"IN HOUGH: "<<endl;
    //  cout<<"         parameter: "<<par_tds.d0()<<" "<<par_tds.phi0()<<" "<<333.567*par_tds.omega()<<" "<<par_tds.z0()<<" "<<par_tds.tanDip()<<endl;
    //  cout<<"         chis: "<<newFit_tds->chisq()<<" ndof: "<<newFit_tds->nDof()<<" nMdc: "<<newFit_tds->nMdc()<<endl;
    double cr= 1./ par_tds.omega();
    double cx= sin(par_tds.phi0()) *(par_tds.d0()+1/par_tds.omega()) * -1.; //z axis verse,x_babar = -x_belle
    double cy= -1.*cos(par_tds.phi0()) *(par_tds.d0()+1/par_tds.omega()) * -1;//???
 
    unsigned bestIndex = 0;
    double   bestDiff = 1.0e+20;
    double   cR, cX, cY;
    vector<double> a0,a1,a2,a3,a4;
    vector<double> zdelta;
    RecMdcTrackCol::iterator iteritrk = trackList->begin();
    int itrk=0;
    for(;iteritrk!=trackList->end();iteritrk++){
      double pt=(*iteritrk)->pxy();
      a0.push_back( (*iteritrk)->helix(0) );
      a1.push_back( (*iteritrk)->helix(1) );
      a2.push_back( (*iteritrk)->helix(2) );
      a3.push_back( (*iteritrk)->helix(3) );
      a4.push_back( (*iteritrk)->helix(4) );
      zdelta.push_back( par_tds.z0()-(*iteritrk)->helix(3) );
      //      cout<<"IN the NEW List: "<<endl;
      //      cout<<"          parameter: "<<a0[itrk]<<"  "<<a1[itrk]<<"  "<<a2[itrk]<<"  "<<a3[itrk]<<"  "<<a4[itrk]<<endl;
      //      cout<<"          chi: "<<(*iteritrk)->chi2()<<" ndof: "<<(*iteritrk)->ndof()<<"nster: "<<(*iteritrk)->nster()<<endl;
      cR=(-333.567)/a2[itrk];
      cX=(cR+a0[itrk])*cos(a1[itrk]);                   
      cY=(cR+a0[itrk])*sin(a1[itrk]);
 
      if(fabs(cr)*(1.-0.25) <= fabs(cR) && fabs(cR) <= fabs(cr)*(1.+0.25)){
    if(fabs(cx-cX) <= fabs(cr)*(0.25)&& fabs(cy-cY) <= fabs(cr)*(0.25) ){
      double diff = fabs((fabs(cr)-fabs(cR))*(cx-cX)*(cy-cY));
      if(diff < bestDiff){
        bestDiff = diff;
        bestIndex = itrk;
      }
    }
      }
      itrk++;
    }
 
    if(bestDiff == 1.0e20) { if(m_debug>0) cout<<" no merge "<<endl; }
    else continue;
 
    //  TrkHitList* hitlist = mytrack.newTrack->hits();
    //  TrkHitList::hot_iterator hotIter= hitlist->begin();
    //  while (hotIter!=hitlist->end()) {
    //    cout <<"(" <<((MdcHit*)(hotIter->hit()))->layernumber()
    //      <<","<<((MdcHit*)(hotIter->hit()))->wirenumber() 
    //      <<":"<<hotIter->isActive()<<")  ";
    //  
    //  if (hotIter->isActive()==0) hitlist->remove() ;
    //  hotIter++;
    //  }
 
 
    MdcTrack* mdcTrack;
    mdcTrack= new MdcTrack(mytrack.newTrack);
    vec_track_in_tds.push_back(mytrack.newTrack);
    int tkStat = 4;//track find by Hough set stat=4
    int tkId = nTdsTk + itrack;
    mdcTrack->storeTrack(tkId, trackList, hitList, tkStat);
    if(m_drawTuple) m_hitOnTrk[outputTrk]=mdcTrack->track().hots()->nActive();
    outputTrk++;
    delete mdcTrack;
 
    double pxy=1/par_tds.omega()/333.567;
    double pz=pxy*par_tds.tanDip();
    double pxyz=sqrt(pxy*pxy+pz*pz);
    if(m_drawTuple){
      m_pt2_rec_tds[itrack]=pxy;
      m_p_rec_tds[itrack]=pxyz;
      m_d0_tds[itrack]=par_tds.d0();
      m_phi0_tds[itrack]=par_tds.phi0();
      m_omega_tds[itrack]=par_tds.omega();
      m_z0_tds[itrack]=par_tds.z0();
      m_tanl_tds[itrack]=par_tds.tanDip();
      m_Q_tds[itrack]=pxy>0?1:-1;
    }
    itrack++;
  }
  if(m_drawTuple) m_outputTrk=outputTrk;
  int nTdsTk_temp=trackList->size();
  if(m_debug>0)  m_mdcPrintSvc->printRecMdcTrackCol();
 
  /*
     m_track_use_intrk1=vec_hit_use_intrack1.size();
     int nNoise_intrk1=0;
     for(int ihit=0;ihit<vec_hit_use_intrack1.size();ihit++){
     int noise_yes = hit_noise.count( vec_hit_use_intrack1[ihit] ); 
     if(noise_yes==1) {nNoise_intrk1++; cout<<"noise hit in track1: "<<vec_hit_use_intrack1[ihit]<<endl;}
     }
     m_noise_intrk1=nNoise_intrk1;
     */
 
 
  //  m_nhitdis1=vec_hit_use.size();
  //  m_nhitdis2=vec_hitbeforefit.size();
  //  for(int ihit=0;ihit<vec_hit_use.size();ihit++){
  //    int hittemp=vec_hit_use[ihit];
  //    m_hit_dis1[ihit]=hit_to_circle2[hittemp];
  //    m_hit_dis1_3d[ihit]=hit_to_circle3[hittemp];
  //  }
  //  for(int ihit=0;ihit<vec_hitbeforefit.size();ihit++){
  //    int hittemp=vec_hitbeforefit[ihit];
  //    m_hit_dis2[ihit]=hit_to_circle2[hittemp];
  //    m_hit_dis2_3d[ihit]=hit_to_circle3[hittemp];
  //  }
 
  for(int i=0;i<vec_for_clean.size();i++){
    delete vec_for_clean[i];
  }
  for(int i=0;i<vec_trk_for_clean.size();i++){
    //cout<<"size "<<vec_trk_for_clean.size()<<endl;
    //cout<<"vec_clean: "<<vec_trk_for_clean[i]<<endl;
    vector<TrkRecoTrk*>::iterator iterTrk =find(vec_track_in_tds.begin(),vec_track_in_tds.end(),vec_trk_for_clean[i]);
    if(iterTrk ==vec_track_in_tds.end() ) delete vec_trk_for_clean[i];
    //for(int j=0;j<vec_newTrack.size();j++) cout<<"vec_newTrack: "<<vec_newTrack[j]<<endl;
  }
  delete h1;
  delete h2;
 
  if(m_drawTuple) ntuplehit->write();
  if(m_debug>0)  cout<<"end event" <<endl;
  return StatusCode::SUCCESS;
}
 
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * 
StatusCode HoughValidUpdate::finalize() {
  MsgStream log(msgSvc(), name());
  delete m_bfield ;
  delete m_context ;
  log << MSG::INFO << "in finalize()" << endreq;
  return StatusCode::SUCCESS;
}
int HoughValidUpdate::digiToHots(MdcDigiVec mdcDigiVec,TrkRecoTrk* newTrack,vector<bool> vec_truthHit,uint32_t getDigiFlag,vector<double> vec_flt_least,map<int,double> hit_to_circle1,vector<MdcHit*>& vec_for_clean){
  TrkHitList* trkHitList = newTrack->hits();
  int digiId=0;
  if(m_debug>0) cout<<"MdcDigiVec(in 2d fit):  "<<mdcDigiVec.size()<<endl;
  MdcDigiVec::iterator iter = mdcDigiVec.begin();
  for (;iter != mdcDigiVec.end(); iter++, digiId++) {
    if(vec_truthHit[digiId]==false) continue;
    const MdcDigi* aDigi = *iter;
    MdcHit *hit = new MdcHit(aDigi, m_gm);
    vec_for_clean.push_back(hit);
    //  MdcHit *hit=MdcHit(aDigi, m_gm);
    Identifier id = aDigi->identify();
    int layer = MdcID::layer(id);
    int wire  = MdcID::wire(id);
    int hittemp=layer*1000+wire;
    if ((layer>=0&&layer<=7)||(layer>=20&&layer<=35)) {continue;}   
    hit->setCalibSvc(m_mdcCalibFunSvc);
    hit->setCountPropTime(true); 
    // new fltLen and ambig
    int newAmbig = 0;
    // calc. position of this point
    MdcRecoHitOnTrack temp(*hit, newAmbig, m_bunchT0*1.e9);//m_bunchT0 nano second here
    MdcHitOnTrack* newhot = &temp;
    double fltLen = m_gm->Layer(layer)->rMid();
    //  newhot->setFltLen(fltLen);
    newhot->setFltLen(vec_flt_least[digiId]);             //caculate by mc
    //newhot->setFltLen(0);
    //newhot->setHitRms(0.02);
    //if(m_debug>1) std::cout<<name()<<" ("<<layer<<","<<wire<<") fltLen "<<newhot->fltLen()<<std::endl;
    double ddCut;
    //if(layer<8) ddCut=0.6;
    //else        ddCut=0.8;
    if(layer<8) ddCut=1.0;
    else        ddCut=1.0;
    int use_in2d=1;
    if(hit->driftDist(m_bunchT0,0)>ddCut||vec_truthHit[digiId]==false/*||vec_theta_ontrack[digiId]>M_PI*/||fabs(hit_to_circle1[hittemp])>10.) { use_in2d=0; }
    if(m_debug>1) std::cout<<" ("<<layer<<","<<wire<<",rt "<<hit->rawTime()<<",dt "<<hit->driftTime(m_bunchT0,0)<<") T0 " << m_mdcCalibFunSvc->getT0(layer,wire) <<" timewalk "<<  m_mdcCalibFunSvc->getTimeWalk(layer, aDigi->getChargeChannel())<<" m_bunchT0 "<<m_bunchT0<<"  hit to circle: "<<hit_to_circle1[hittemp]<<"  dist: "<<hit->driftDist(m_bunchT0,0)<<"  use?: "<<use_in2d<<endl;
    //if(hit->driftTime(m_bunchT0,0)>800) continue; 
    if(use_in2d==0) continue;
    trkHitList->appendHot(newhot);
    //delete hit;
  }
  if(m_debug>0) std::cout<<std::endl;
  return trkHitList->nHit();
}
 
double HoughValidUpdate::CFtrans(double x,double y){
  return 2*x/(x*x+y*y);
}
 
int HoughValidUpdate::SlantId(int layer){
  if ((layer>=0&&layer<=7)||(layer>=20&&layer<=35)) { 
    if ((layer>=0&&layer<=3)||(layer>=20&&layer<=23)||(layer>=28&&layer<=31)){
      //      m_slant[digiId]=-1;
      return -1;
    }else{ 
      return 1;
    }
  } else{
    return 0;
  }
}
 
int HoughValidUpdate::GetMcInfo(){
  MsgStream log(msgSvc(), name());
  StatusCode sc;
  SmartDataPtr<McParticleCol> mcParticleCol(eventSvc(),"/Event/MC/McParticleCol");
  if (!mcParticleCol) {
    log << MSG::ERROR << "Could not find McParticle" << endreq;
    return -999;
  }
 
  int itk = 0;
  int t_qTruth = 0;                    //zhangjin
  int t_pidTruth = -999;
  int t_McTkId = -999;
  Helix* mchelix;
  vector<int> vec_decay;
  McParticleCol::iterator iter_mc = mcParticleCol->begin();
  for (;iter_mc != mcParticleCol->end(); iter_mc++ ) {
    //  if(!(*iter_mc)->primaryParticle()) continue;
    t_pidTruth = (*iter_mc)->particleProperty();
    bool t_decay= (*iter_mc)->decayInFlight();
    if(m_debug>0) cout<<"Decay : "<<t_decay<<endl;
    vec_decay.push_back(t_decay);
 
    if(m_debug>2) cout<< "tk "<<itk<< " pid="<< t_pidTruth<< endl;
    if((m_pid!=0) && (t_pidTruth != m_pid)){ continue; }                  //zhangjin
    if(itk>=10) break;
 
    int pid = t_pidTruth; 
    if( pid == 0 ) { 
      log << MSG::WARNING << "Wrong particle id" <<endreq;
    }else{
      IPartPropSvc* p_PartPropSvc;
      static const bool CREATEIFNOTTHERE(true);
      StatusCode PartPropStatus = service("PartPropSvc", p_PartPropSvc, CREATEIFNOTTHERE);
      if (!PartPropStatus.isSuccess() || 0 == p_PartPropSvc) {
    std::cout<< " Could not initialize Particle Properties Service" << std::endl;
      }
      HepPDT::ParticleDataTable* p_particleTable = p_PartPropSvc->PDT(); 
      std::string name;
      if( p_particleTable->particle(pid) ){
    name = p_particleTable->particle(pid)->name();
    t_qTruth = p_particleTable->particle(pid)->charge();
      }else if( p_particleTable->particle(-pid) ){
    name = "anti " + p_particleTable->particle(-pid)->name();
    t_qTruth = (-1)*p_particleTable->particle(-pid)->charge();
      }
      if(m_debug>2) std::cout << " particle "<<name<<" charge= "<<t_qTruth<<std::endl;
    }
 
    t_McTkId = (*iter_mc)->trackIndex();
    double px = (*iter_mc)->initialFourMomentum().px();//GeV
    double py = (*iter_mc)->initialFourMomentum().py();//GeV
    double pz = (*iter_mc)->initialFourMomentum().pz();//GeV
 
 
    mchelix = new Helix((*iter_mc)->initialPosition().v(), (*iter_mc)->initialFourMomentum().v(), t_qTruth);//cm
    mchelix->pivot( HepPoint3D(0.,0.,0.) );
    //cout<<"pt, p : "<<mchelix->pt()<<" "<<mchelix->momentum()<<endl;
 
    if(m_debug>2){
      std::cout<<"Truth tk "<<itk<<" pid "<<pid<<" charge "<<t_qTruth
    << " helix "<< mchelix->a()<<" p "<<mchelix->momentum(0.)<<std::endl;
    }
    if(m_drawTuple){
      m_pzTruth[itk]=pz;
      m_pidTruth[itk] = t_pidTruth;
      m_costaTruth[itk] = mchelix->cosTheta();
      m_ptTruth[itk] = mchelix->pt();
      m_pTruth[itk] = mchelix->momentum(0.).mag();
      m_qTruth[itk] = t_qTruth;
      m_drTruth[itk] = mchelix->dr();
      m_phi0Truth[itk] = mchelix->phi0();
      m_omegaTruth[itk]  =1./(mchelix->radius());         //Q
      m_dzTruth[itk] = mchelix->dz();
      m_tanl_mc[itk]  =mchelix->tanl();
    }
    itk++;
    if(m_debug>2){
      cout<<"     d0:  "   <<"  "<<mchelix->dr()<<endl;
      cout<<"     phi0:  " <<"   "<<mchelix->phi0()<<endl;
      cout<<"     omega:  "<<"  "<<1./(mchelix->radius())<<endl;
      cout<<"     z0:   "  <<"  "<<mchelix->dz()<<endl;
      cout<<"     tanl:   "<<"   "<<mchelix->tanl()<<endl;
      cout<<"     pt:   "  <<"   "<<mchelix->pt()<<endl;            //Q
      cout<<"     costaTruth:   "  <<"   "<<mchelix->cosTheta()<<endl;            //Q
    }
    delete mchelix;
  }
  if(m_drawTuple){
    vector<int>::iterator iter_idecay=find(vec_decay.begin(),vec_decay.end(),1 );
    if (iter_idecay==vec_decay.end() ) m_decay=0;
    else m_decay=1;
    m_nTrkMC     = itk;
  }
  if(itk!=1) {
    if(m_debug>2)   std::cout<<"WARNING run "<<t_runNum<<" evt "<<t_eventNum<<" not single event. nTrkMc="<<itk<<std::endl;
    return -1;
  }else{
    if(m_debug>2) std::cout<<"nTrkMc=1"<<std::endl;
    return  1;
  }
 
}
 
 
int HoughValidUpdate::LeastSquare(int n,vector<double> vec_x,vector<double> vec_y, vector<int> vec_slant,vector<int> vec_layer,vector<bool> vec_truthHit,double &d0,double &phi0, double &omega,double& circleX,double& circleY,double& circleR){
 
  double x_sum=0;
  double y_sum=0;
  double x2_sum=0;
  double y2_sum=0;
  double x3_sum=0;
  double y3_sum=0;
  double x2y_sum=0;
  double xy2_sum=0;
  double xy_sum=0;
  double a1=0;
  double a2=0;
  double b1=0;
  double b2=0;
  double c1=0;
  double c2=0;
  double x_aver,y_aver,r2;
  int use_in_least=0;
  for(int i=0;i<n;i++){
    if(vec_truthHit[i]==false) continue;
    if(vec_slant[i]!=0) continue;
    x_sum=x_sum+vec_x[i];
    y_sum=y_sum+vec_y[i];
    x2_sum=x2_sum+vec_x[i]*vec_x[i];
    y2_sum=y2_sum+vec_y[i]*vec_y[i];
    x3_sum=x3_sum+vec_x[i]*vec_x[i]*vec_x[i];
    y3_sum=y3_sum+vec_y[i]*vec_y[i]*vec_y[i];
    x2y_sum=x2y_sum+vec_x[i]*vec_x[i]*vec_y[i];
    xy2_sum=xy2_sum+vec_x[i]*vec_y[i]*vec_y[i];
    xy_sum=xy_sum+vec_x[i]*vec_y[i];
    use_in_least++;
  }
  a1=x2_sum-x_sum*x_sum/n;
  a2=xy_sum-x_sum*y_sum/n;
  b1=a2;
  b2=y2_sum-y_sum*y_sum/n;
  c1=(x3_sum+xy2_sum-x_sum*(x2_sum+y2_sum)/n)/2.;
  c2=(y3_sum+x2y_sum-y_sum*(x2_sum+y2_sum)/n)/2.;
 
  x_aver=x_sum/n;
  y_aver=y_sum/n;
 
  double  x_cirtemp =(b1*c2-b2*c1)/(b1*b1-a1*b2);
  double  y_cirtemp =(b1*c1-a1*c2)/(b1*b1-a1*b2);
  r2=(x2_sum+y2_sum-2*x_cirtemp*x_sum-2*y_cirtemp*y_sum)/n+x_cirtemp*x_cirtemp+y_cirtemp*y_cirtemp; 
  double r_temp=sqrt(r2);
 
  circleX = x_cirtemp;
  circleY = y_cirtemp;
  circleR = r_temp;
 
  double d0_temp=sqrt(x_cirtemp*x_cirtemp+y_cirtemp*y_cirtemp)-r_temp;
  double  pt_temp=r_temp/333.567;
  if(m_debug>0){
    cout<<"  in  least fit :  "<<endl;
    cout<<"rtemp:   "<<r_temp<<"  "<<endl;
    cout<<"xtemp:   "<<x_cirtemp<<"  "<<endl;
    cout<<"ytemp:   "<<y_cirtemp<<"  "<<endl;
    cout<<"d0temp:  "<<d0_temp<<"    "<<endl;
    cout<<"pt_temp: "<<pt_temp<<endl;
  }
 
  d0=d0_temp;            //opposite with Q 
  //phi0=atan2(y_cirtemp,x_cirtemp)-(m_q)*M_PI/2.;          //charge- + ; charge+ -;
  phi0=atan2(y_cirtemp,x_cirtemp)+M_PI/2.;          //charge- + ; charge+ -;
  omega=1/r_temp;               //according with Q
  double z0=0.;
  double tanl=0.;
  if(m_debug>0) std::cout<<" before global fit  Helix PatPar :"<<d0<<","<<phi0<<","<<omega<<","<<z0<<","<<tanl<<  std::endl;
  return 0;
 
} 
 
int HoughValidUpdate::Zposition(int n,vector<int> vec_slant,double x_cirtemp,double y_cirtemp,double r_temp,vector<double> vec_x_east,vector<double> vec_x_west,vector<double> vec_y_east,vector<double> vec_y_west,vector<double> vec_z_east,vector<double> vec_z_west,vector<int> vec_layer,vector<int> vec_wire,vector<double> vec_z,vector<double>& vec_zStereo,vector<double>& l,vector<bool> vec_truthHit){
  double x_stereo;
  double y_stereo;
  //  double theta;
  int stereoId=0;
  //vector<double> z_stereo;
  // vector<double> vec_zStereo;
  int nDropDelta=0;
  for(int i=0;i<n;i++){
    if(vec_truthHit[i]==false) continue;
    double k;
    double b;
    if(vec_slant[i]!=0){
      if(vec_x_east[i]!=vec_x_west[i]){
    k=(vec_y_east[i]-vec_y_west[i])/(vec_x_east[i]-vec_x_west[i]);
    b=-k*vec_x_east[i]+vec_y_east[i];
    //    cout<<"eventNum: "<<t_eventNum<<" "<<"layerid: "<<" "<<m_layer[i]<<"cellid: "<<m_cell[i]<<" "<<"k: "<<k<<" "<<"b: "<<b<<" "<<m_y_east[i]<<"  "<<m_y_west[i]<<"  "<<m_x_east[i]<<"  "<<m_x_west[i]<<"  "<<endl;
    double delta=4*(k*b-k*y_cirtemp-x_cirtemp)*(k*b-k*y_cirtemp-x_cirtemp)-4*(k*k+1)*(x_cirtemp*x_cirtemp+b*b+y_cirtemp*y_cirtemp-2*b*y_cirtemp-r_temp*r_temp);
    //cout<<"layer:  "<<vec_layer[i]<<"wire:  "<<vec_wire[i]<<endl;
    //cout<<"x_east:  "<<vec_x_east[i]<<"  "<<"x_west:   "<<vec_x_west[i]<<endl;
    //cout<<"k:   "<<k<<"  "<<"b:  "<<b<<endl;
    //cout<<"x_cirtemp:  "<<x_cirtemp<<"  "<<"y_cirtemp:  "<<y_cirtemp<<endl;
    if(delta<0) {
      nDropDelta++;
      continue;
    }
    //cout<<"delta:     "<<delta<<endl;
    double x1=(2*x_cirtemp+2*k*y_cirtemp-2*k*b+sqrt(delta))/(2*(k*k+1));
    double x2=(2*x_cirtemp+2*k*y_cirtemp-2*k*b-sqrt(delta))/(2*(k*k+1));
    double x_middle=(vec_x_east[i]+vec_x_west[i])/2.;
    //    if(abs(x1-vec_x[i])>abs(x2-vec_x[i])) {x_stereo=x2;} 
    if(abs(x1-x_middle)>abs(x2-x_middle)) {x_stereo=x2;} 
    else {x_stereo=x1;}
    y_stereo=k*x_stereo+b;
      }
      else{
    //cout<<"layer:  "<<vec_layer[i]<<"wire:  "<<vec_wire[i]<<endl;
    //cout<<"x_east:  "<<vec_x_east[i]<<"  "<<"x_west:   "<<vec_x_west[i]<<endl;
    x_stereo=vec_x_east[i];
    double y1=sqrt(r_temp*r_temp-(x_cirtemp-x_stereo)*(x_cirtemp-x_stereo))+y_cirtemp;
    double y2=y_cirtemp-sqrt(r_temp*r_temp-(x_cirtemp-x_stereo)*(x_cirtemp-x_stereo));
    double y_middle=(vec_y_east[i]+vec_y_west[i])/2.;
    if(abs(y1-y_middle)>abs(y2-y_middle)) {y_stereo=y2;}
    else{y_stereo=y1;}
      }
      //      cout<<"layer:  "<<vec_layer[i]<<"wire:  "<<vec_wire[i]<<"x_stereo:  "<<x_stereo<<"  "<<"y_stereo:  "<<y_stereo<<endl;
 
      double theta_temp;
      if(x_cirtemp==0||x_stereo-x_cirtemp==0){
    theta_temp=0;   
      }
      else{
    theta_temp=M_PI-atan2(y_stereo-y_cirtemp,x_stereo-x_cirtemp)+atan2(y_cirtemp,x_cirtemp);
    if(theta_temp>2*M_PI){
      theta_temp=theta_temp-2*M_PI;
    }
    if(theta_temp<0){
      theta_temp=theta_temp+2*M_PI;
    }
      }
      //cout<<"thetatemp:   "<<theta_temp<<endl;
      //if(theta_temp>2*M_PI) {
      //  theta_temp=theta_temp-2*M_PI;
      //}
      //if(theta_temp<0.) {
      //  theta_temp=theta_temp+2*M_PI;
      //}
      //cout<<"debug: if thetatemp is cout over"<<endl;
      l.push_back(r_temp*theta_temp);
      //cout<<"debug: if l is pushback"<<endl;
      //l.push_back(M_PI-atan2(y_stereo,x_stereo)-atan2(y_cirtemp,x_cirtemp));
      //l.push_back(r_temp*(M_PI-atan2(y_stereo,x_stereo)-atan2(y_cirtemp,x_cirtemp)));
 
      double d1=sqrt((x_stereo-vec_x_west[i])*(x_stereo-vec_x_west[i])+(y_stereo-vec_y_west[i])*(y_stereo-vec_y_west[i]));
      double d2=sqrt((vec_x_east[i]-vec_x_west[i])*(vec_x_east[i]-vec_x_west[i])+(vec_y_east[i]-vec_y_west[i])*(vec_y_east[i]-vec_y_west[i]));
      vec_zStereo.push_back(vec_z_west[i]-(vec_z_west[i]-vec_z_east[i])*d1/d2);
      //cout<<"debug: if vec_zStereo is pushback"<<endl;
      //if(stereoId==0) {vec_zStereo.push_back(vec_z_west[i]-(vec_z_west[i]-vec_z_east[i])*d1/d2);}
      //if(stereoId>0){
      //  if(vec_layer[i]==vec_layer.at(i-1)) {
      //    vec_zStereo[(stereoId-1)]=((z_stereo.at(stereoId)+z_stereo.at(stereoId-1))/2.);
      //    vec_zStereo.push_back((z_stereo.at(stereoId)+z_stereo.at(stereoId-1))/2.);
      //  }
      //  else{
      //    vec_zStereo.push_back(vec_z_west[i]-(vec_z_west[i]-vec_z_east[i])*d1/d2);
      //  }
      //}
 
      //      double delphi=atan2(y_stereo,x_stereo)-par.phi0();
      //cout<<"eventNum: "<<t_eventNum<<" "<<"layerid: "<<" "<<m_layer[i]<<"cellid: "<<" "<<m_cell[i]<<" "<<"z_stereo: "<<z_stereo<<"  "<<d1<<"   "<<d2<<"    "<<endl;
      //cout<<"eventNum: "<<t_eventNum<<" "<<"layerid: "<<" "<<m_layer[i]<<"cellid: "<<m_cell[i]<<" "<<"k: "<<k<<" "<<"b: "<<b<<" "<<m_y_east[i]<<"  "<<m_y_west[i]<<"  "<<m_x_east[i]<<"  "<<m_x_west[i]<<"  "<<"x_stereo:  "<<"  "<<x_stereo<<"  "<<"x:  "<<vec_x[i]<<"y_stereo: "<<" "<<y_stereo<<"  "<<"y:  "<<vec_y[i]<<"                  "<<"zstereo:  "<<z_stereo<<endl;
      //      cout<<"layerid: "<<"   "<<vec_layer[i]<<"   "<<"cellid: "<<vec_wire[i]<<"    "<<"x_stereo:  "<<"   "<<x_stereo<<"  "<<"y_stereo: "<<"  "<<y_stereo<<"    "<<"zstereo:  "<<z_stereo.at(stereoId)<<"   "<<vec_zStereo.at(stereoId)<<"    "<<"ztruth:  "<<vec_z[i]<<endl;
 
      //      cout<<"layer:  "<<vec_layer[i]<<"wire:  "<<vec_wire[i]<<"z:  "<<vec_zStereo[stereoId]<<"  "<<"l:  "<<l[stereoId]<<"  "<<vec_truthHit[i]<<endl;
      stereoId=stereoId+1;
      //cout<<"theta:  "<<theta<<endl;
    }   
    else {k=b=0;}
  }
  cout<<"nDropDelta:  "<<nDropDelta<<endl;
  //  cout<<"if for is finished :  "<<endl;
  //int digiId=0;
  //for(int i=0;i<n;i++){
  //  if(vec_slant[i]!=0){
  //    cout<<"layerid: "<<"   "<<vec_layer[i]<<"   "<<"cellid: "<<vec_wire[i]<<"    "<<"zstereo:  "<<z_stereo.at(digiId)<<"   "<<vec_zStereo.at(digiId)<<"    "<<"ztruth:  "<<vec_z[i]<<endl;
  //    digiId++;
  //  } 
  //}
  //double sum_zstereo=0;
  //for(int i=0;i<stereoId;i++){
  //  sum_zstereo=sum_zstereo+z_stereo[i];
  //}
  //cout<<"sum_zstereo:  "<<sum_zstereo<<endl;
  return stereoId;
}
 
 
void HoughValidUpdate::Linefit(int z_stereoNum,vector<double> vec_zStereo,vector<double> l,double& tanl,double& z0){
 
  double l_sum=0;
  double z_sum=0;
  double l2_sum=0;
  double z2_sum=0;
  double lz_sum=0;
  for(int i=0;i<z_stereoNum;i++){
    //  cout<<"event:  "<<t_eventNum<<"  "<<"z:  "<<vec_zStereo[i]<<"  "<<"l:  "<<l[i]<<endl;
    l_sum=l_sum+l[i];
    z_sum=z_sum+vec_zStereo[i];
    l2_sum=l2_sum+l[i]*l[i];
    //  cout<<l2_sum<<endl;
    z2_sum=z2_sum+vec_zStereo[i]*vec_zStereo[i];
    lz_sum=lz_sum+l[i]*vec_zStereo[i];
  }
  double b=(l2_sum*z_sum-lz_sum*l_sum)/(z_stereoNum*l2_sum-l_sum);
  double k=(z_stereoNum*lz_sum-l_sum*z_sum)/(z_stereoNum*l2_sum-l_sum*l_sum);
  z0=b;
  tanl=k;
  cout<<k<<"  "<<b<<endl;
}
 
 
 
int HoughValidUpdate::digiToHots2(MdcDigiVec mdcDigiVec,TrkRecoTrk* newTrack,vector<bool> vec_truthHit,vector<double> vec_flt_truth,uint32_t getDigiFlag, vector<int> vec_slant, vector<double> vec_l_Calcu,vector<double> vec_flt,vector<double> vec_theta_ontrack,vector<int>& vec_hitbeforefit,map<int,double> hit_to_circle2,vector<MdcHit*>& vec_for_clean,double tanl){
  TrkHitList* trkHitList = newTrack->hits();
  //MdcDigiVec mdcDigiVec = m_rawDataProviderSvc->getMdcDigiVec(getDigiFlag);
  if(m_debug>0) cout<<"MdcDigiVec(in 3d fit):  "<<mdcDigiVec.size()<<endl;
  MdcDigiVec::iterator iter1 = mdcDigiVec.begin();
  int digiId=0;
  int nhit_beforefit_temp=0;
  for (;iter1 != mdcDigiVec.end(); iter1++, digiId++) {
    //cout<<"fltcut: "<<m_fltcut<<endl;
    if(vec_theta_ontrack[digiId]>M_PI) continue;
    //  if(digiId>20&&vec_slant[digiId]!=0) continue;
    const MdcDigi* aDigi = *iter1;
    MdcHit *hit = new MdcHit(aDigi, m_gm);
    vec_for_clean.push_back(hit);
    //MdcHit *hit=MdcHit(aDigi, m_gm);
    Identifier id = aDigi->identify();
    int layer = MdcID::layer(id);
    int wire  = MdcID::wire(id);
    int hittemp=layer*1000+wire; 
    hit->setCalibSvc(m_mdcCalibFunSvc);
    hit->setCountPropTime(true); //hit->setCountPropTime(m_countPropTime);
 
    // new fltLen and ambig
    int newAmbig = 0;
    // calc. position of this point
    MdcRecoHitOnTrack temp(*hit, newAmbig, m_bunchT0*1.e9);//m_bunchT0 nano second here
    MdcHitOnTrack* newhot = &temp;
    double fltLen = m_gm->Layer(layer)->rMid();
    //  newhot->setFltLen(fltLen);
    newhot->setFltLen(vec_flt[digiId]);             
    //if (vec_slant[digiId]==0) {newhot->setFltLen(vec_flt[digiId]);/* cout<<" ("<<layer<<","<<wire<<") "<<"l_truth: "<<vec_flt_truth[digiId]<<" l_axil: "<<vec_flt[digiId]<<endl;*/}             //caculate by mc
    //  else                      {newhot->setFltLen(vec_l_Calcu[digiId]);/* cout<<" ("<<layer<<","<<wire<<") "<<" l_truth: "<<vec_flt_truth[digiId]<<" l_Calcu: "<<vec_l_Calcu[digiId]<<endl;*/}             //caculate by mc
    //  newhot->setActivity(1);
    //bool active = newhot->isActive();
    //newhot->setHitRms(0.02);
    //if(m_debug>1) std::cout<<name()<<" ("<<layer<<","<<wire<<") fltLen "<<newhot->fltLen()<<std::endl;
    double ddCut;
    double distcirCut;
    //if(layer<8) {ddCut=0.8; distcirCut=5;}
    //else        {ddCut=0.6; distcirCut=2;}
    if(layer<8) {ddCut=1.0; distcirCut=5;}
    else        {ddCut=1.0; distcirCut=2;}
    int use_in3d=1;
    if(hit->driftDist(m_bunchT0,0)>ddCut||vec_truthHit[digiId]==false||/*vec_theta_ontrack[digiId]>M_PI||*/fabs(hit_to_circle2[hittemp])>distcirCut) { use_in3d=0; }
    if(m_debug>2) std::cout<<" ("<<layer<<","<<wire<<",rt "<<hit->rawTime()<<",dt "<<hit->driftTime(m_bunchT0,0)<<") T0 " << m_mdcCalibFunSvc->getT0(layer,wire) <<" timewalk "<<  m_mdcCalibFunSvc->getTimeWalk(layer, aDigi->getChargeChannel())<<" vec_truth: "<<vec_truthHit[digiId]<<" theta: "<<vec_theta_ontrack[digiId]<<" ddcut?: "<<hit->driftDist(m_bunchT0,0)<<" distocir: "<<hit_to_circle2[hittemp]<<"  use:? "<<use_in3d<<endl;
    if(use_in3d==0) continue;
 
    vec_hitbeforefit.push_back(hittemp);
    trkHitList->appendHot(newhot);
  }
  if(m_debug>0) std::cout<<std::endl;
  return trkHitList->nHit();
}
void HoughValidUpdate:: Multi_array(int binX,int binY){
  int **count=new int*[binX];
  for(int i=0;i<binX;i++){
    count[i]=new int [binY];
  }
  //vector<int> vec_selectNum[binX][binY];
  int ***selectNum=new int**[binX];
  for(int i=0;i<binX;i++){
    selectNum[i]=new int* [binY];
    for(int j=0;j<binY;j++){
      //selectNum[i][j]=new int[];
      selectNum[i][j]=new int [count[i][j]];
    }
  }
}
 
void HoughValidUpdate::RhoTheta(int nhit,vector<double> vec_u,vector<double> vec_v,vector<double>& vec_rho,vector<double>& vec_theta,vector< vector<int> >& vec_hitNum,vector<int> vec_digitruth){
  for(int i=0;i<nhit;i++){
    if(vec_digitruth[i]==0) continue;
    for(int j=i+1;j<nhit;j++){
      if(vec_digitruth[j]==0) continue;
      double k,b,x_cross,y_cross;
      double rho_temp,theta_temp; 
      k=(vec_v[i]-vec_v[j])/(vec_u[i]-vec_u[j]);
      b=vec_v[i]-k*vec_u[i];
      x_cross=-b/(k+1/k);
      y_cross=b/(k*k+1);
      rho_temp=sqrt(x_cross*x_cross+y_cross*y_cross);
      theta_temp=atan2(y_cross,x_cross);
      if(theta_temp<0)  {
    theta_temp=theta_temp+M_PI;
    rho_temp=-rho_temp;
      }
      vec_rho.push_back(rho_temp);
      vec_theta.push_back(theta_temp);
      vec_hitNum[0].push_back(i);
      vec_hitNum[1].push_back(j);
      //cout<<"u:"<<vec_u[i]<<" "<<"v:"<<vec_v[i]<<"  "<<"("<<vec_theta[i]<<","<<vec_rho[i]<<")"<<endl;
    }
  }
  int nCross=vec_rho.size();
  m_nCross=vec_rho.size();
  if(m_nCross>125000) return ;
  //  cout<<"numCross: "<<nCross<<endl;
 
  for(int i=0;i<nCross;i++){
    m_rho[i]=vec_rho[i];
    m_theta[i]=vec_theta[i];
    //cout<<" "<<vec_theta[i]<<" "<<vec_rho[i]<<endl;
  } 
}
void HoughValidUpdate::RhoThetaAxial(vector<int> vec_slant,int nhit,vector<double> vec_u,vector<double> vec_v,vector<double>& vec_rho,vector<double>& vec_theta,vector< vector<int> >& vec_hitNum,vector<int> vec_digitruth){
  for(int i=0;i<nhit;i++){
    if(vec_digitruth[i]==0) continue;
    if(vec_slant[i]!=0) continue;
    for(int j=i+1;j<nhit;j++){
      if(vec_slant[j]!=0) continue;
      if(vec_digitruth[j]==0) continue;
      double k,b,x_cross,y_cross;
      double rho_temp,theta_temp; 
      k=(vec_v[i]-vec_v[j])/(vec_u[i]-vec_u[j]);
      b=vec_v[i]-k*vec_u[i];
      x_cross=-b/(k+1/k);
      y_cross=b/(k*k+1);
      rho_temp=sqrt(x_cross*x_cross+y_cross*y_cross);
      theta_temp=atan2(y_cross,x_cross);
      if(theta_temp<0)  {
    theta_temp=theta_temp+M_PI;
    rho_temp=-rho_temp;
      }
      vec_rho.push_back(rho_temp);
      vec_theta.push_back(theta_temp);
      vec_hitNum[0].push_back(i);
      vec_hitNum[1].push_back(j);
      //cout<<"u:"<<vec_u[i]<<" "<<"v:"<<vec_v[i]<<"  "<<"("<<vec_theta[i]<<","<<vec_rho[i]<<")"<<endl;
    }
  }
  int nCross=vec_rho.size();
  if( m_drawTuple ) m_nCross=vec_rho.size();
 
  if(m_drawTuple){
    for(int i=0;i<nCross;i++){
      m_rho[i]=vec_rho[i];
      m_theta[i]=vec_theta[i];
    }   
  }
 
}
void HoughValidUpdate::FillHist(TH2D *h1,vector<double> vec_u,vector<double> vec_v,int nhit,vector< vector < vector< int > > > &vec_selectNum){
  for(int n=0;n<nhit;n++){
    //  if(vec_digitruth[n]==0) continue;
    for(int i=0;i<binX;i++){
      double binwidth=M_PI/binX;
      double binhigh=2*m_maxrho/binY;
      double theta=(i+0.5)*binwidth;
      double rho=vec_u[n]*cos(theta)+vec_v[n]*sin(theta);
      int j=(int)((rho+m_maxrho)/binhigh);
      int count=h1->GetBinContent(i+1,j+1);
      count+=1;
      h1->SetBinContent(i+1,j+1,count);
      //  h1->Fill(theta,rho);
      vec_selectNum[i][j].push_back(n);
    }
  }
}
void HoughValidUpdate::FillRhoTheta(TH2D *h1 ,vector<double> vec_theta,vector<double> vec_rho){
  for(int i=0;i<vec_theta.size();i++){
    double binwidth=M_PI/binX;
    double binhigh=2*m_maxrho/binY;
    int binxNum=vec_theta[i]/binwidth+1;
    int binyNum=(vec_rho[i]+m_maxrho)/binhigh+1; 
    int count =h1->GetBinContent(binxNum,binyNum);
    count++;
    h1->SetBinContent(binxNum,binyNum,count);
  }
}
//int HoughValidUpdate::zPosition(MdcHitUse & hitUse, const TrkExchangePar &par, MdcLine* span,int hitFit, double t0, double Bz) const
 
int HoughValidUpdate::zPosition(MdcHit& h, double t0, double x_cirtemp,double y_cirtemp, double r_temp,int digiId,double& delta_z,double& delta_l,double& l_Calcu_Left,double& l_Calcu_Right,double& z_Calcu_Left,double& z_Calcu_Right,double& z_Calcu,double& l_Calcu,int& i_q) {
  //const MdcHit &  h = *(hitUse.mdcHit());
 
  //ambig not sure
  //int ambig = hitUse.ambig();
  int return_d[2];
  int return_ok[2];
  return_d[0]=1;
  return_d[1]=1;
  return_ok[0]=1;
  return_ok[1]=1;
  for(int ambig=-1;ambig<=1;ambig=ambig+2){
    //int ambig =m_postruth[digiId];
    //if(ambig==0) ambig =-1;
    HepPoint3D fp ( h.wire()->getWestPoint()->x(),h.wire()->getWestPoint()->y(),h.wire()->getWestPoint()->z());
    HepPoint3D rp ( h.wire()->getEastPoint()->x(),h.wire()->getEastPoint()->y(),h.wire()->getEastPoint()->z());
 
 
    HepVector3D X = 0.5 * (fp + rp);
    if(m_debug>0){
      std::cout<<"----------  "<<std::endl;//yzhang debug
      h.print(std::cout);
      //h.wire()->print(std::cout);
      std::cout<<"fp rp "<<fp<<" "<<rp<<std::endl;//yzhang debug
      std::cout<<"Xmid  "<<X<<std::endl;//yzhang debug
    }
    HepVector3D xx = HepVector3D(X.x(), X.y(), 0.);
    //center of helix/circle
    //change to belle param
 
    /*
       double d0 = -par.d0();
       double phi0 = (par.phi0()-pi/2);
       double _charge = -1;
       if((-1)*par.omega()/Bz > 0) _charge = 1;  
       */
 
    //d0temp,phi0,omega,x_cirtemp,y_cirtemp, r_temp,m_q
    //double r;
    //if (fabs(par.omega())< Constants::epsilon) r = 9999.;
    //else r = 1/par.omega();    //+ -
 
    //double xc = sin(par.phi0()) *(-d0+r) * -1.; //z axis verse,x_babar = -x_belle
    //double yc = -1.*cos(par.phi0()) *(-d0+r) * -1;//???
    //HepPoint3D center (xc, yc, 0. );
    HepPoint3D center (x_cirtemp, y_cirtemp, 0. );
 
    HepVector3D yy = center - xx;
    HepVector3D ww = HepVector3D(yy.x(), yy.y(), 0.);
    double wwmag2 = ww.mag2();
    double wwmag = sqrt(wwmag2);
 
 
    //dirftDist 
    double driftdist = fabs( h.driftDist(t0,ambig) );
    HepVector3D lr(driftdist/wwmag * ww.x(),
    driftdist/wwmag * ww.y(), 0.);
    if (m_debug >4){
      std::cout<<"xc "<<x_cirtemp << " yc "<<y_cirtemp<< " drfitdist "<<driftdist<<std::endl;
      //std::cout<<"r1 "<<r <<" d0 "<<d0 <<" phi0 "<<phi0<<std::endl;//
      //std::cout<<"lr "<<lr<<" hit ambig "<<hitUse.ambig()<<" ambig "<<ambig
      std::cout<<"lr "<<lr<<"     "<<" ambig "<<ambig
    <<" left "<<h.driftDist(0,1)
    <<" right "<<h.driftDist(0,-1)<<std::endl;
    }
 
    //...Check left or right...
    HepPoint3D ORIGIN = HepPoint3D(0., 0., 0.);
    //ambig
    //-1 right -phi direction driftDist-=lr 
    //, +1 left +phi directon driftDist+=lr
    if (ambig == 0) lr = ORIGIN;
    //  cout<<"m_q: "<<m_q<<"  i_q: "<<i_q<<endl;
    if (i_q> 0){ //yzhang
      if (ambig == -1){
    lr = - lr;//right
      }
    }else{
      if (ambig == 1){
    lr = - lr;//left
      }
    }
    X += lr;
 
    //...Prepare vectors...
    //    HepPoint3D center = _helix->center();
    HepPoint3D tmp(-9999., -9999., 0.);
    HepVector3D x = HepVector3D(X.x(), X.y(), 0.);
    HepVector3D w = x - center;
    // //modified the next sentence because the direction are different from belle.
    HepVector3D V = h.wire()->zAxis();
    HepVector3D v = HepVector3D(V.x(), V.y(), 0.);
    double vmag2 = v.mag2();
    //double vmag = sqrt(vmag2);
 
    //double r = _helix->curv();
    double wv = w.dot(v);
    //    wv = abs(wv);
    double d2 = wv * wv - vmag2 * (w.mag2() - r_temp * r_temp);
    if (m_debug >4){
      std::cout<<"X_fix "<<X<<" center "<<center<<std::endl;
      std::cout<<"V "<<V<<std::endl;//yzhang debug
      std::cout<<"w "<<w<<" v "<<v<<std::endl;      
      std::cout<<"wv "<<wv<<endl;
      cout<<"d2: "<<d2<<endl;
    }
    //...No crossing in R/Phi plane... This is too tight...
 
    if (d2 < 0.) {
      //hitUse.position(tmp);
      if (m_debug>4){
    cout<<"d2: "<<d2<<endl;
    std::cout << "in MdcSegInfoSterO !!! stereo: 0. > d2 = " << d2 << " "
      << ambig << std::endl;
      }
      //return -1;
      if (ambig==-1) return_d[ambig+1]=0;
      else           return_d[ambig]=0;
      continue;
    }
    double d = sqrt(d2);
    //  cout<<"d:   "<<d<<endl;
 
 
    double l[2];
    l[0] =-1*( (- wv + d) / vmag2 );          //multiply -1  ......?
    l[1] =-1*( (- wv - d) / vmag2 );
 
    double z[2];
    //z[0] = X.z() + l[0] * V.z();               
    z[0] = X.z() - l[0] * V.z();               //l *-1
    z[1] = X.z() - l[1] * V.z();
 
 
    //...Cal. length to crossing points...
    // double l[2];
    //if (debug >0){
    //std::cout<<"wv "<<wv<<" d "<<d<<" vmag2 "<<vmag2<<std::endl;//yzhang debug
    //}
    //  l[0] = (- wv + d) / vmag2;
 
    //  l[1] = (- wv - d) / vmag2;
 
    //...Cal. z of crossing points...
    bool ok[2];
    ok[0] = true;
    ok[1] = true;
    //double z[2];
    // z[0] = X.z() + l[0] * V.z();
    //z[1] = X.z() + l[1] * V.z();
    if (m_debug >4){//yzhang m_debug
      std::cout << "X.z  "<<X.z()<<" V.z "<<V.z()<<std::endl;
      std::cout << "l0, l1 = " << l[0] << ", " << l[1] << std::endl;
      std::cout << "rpz " << rp.z() << " fpz " << fp.z() << std::endl;
      std::cout << "z0  " << z[0] << " z1  " << z[1] << std::endl;
    }
    //...Check z position...
    if (ambig == 0) {
      if(m_debug>4)std::cout<< " ambig = 0  " <<std::endl;
      if (z[0] > rp.z()+20.  || z[0] < fp.z()-20.) {
    ok[0] = false;
      }
      if (z[1] > rp.z()+20.  || z[1] < fp.z()-20.) {
    ok[1] = false;
      }
    } else {
      if(m_debug>4)std::cout<< " ambig != 0  " <<std::endl;
      // if (z[0] > rp.z() || z[0] < fp.z() ) 
      if (fabs(z[0]/rp.z())>1.05 ) { ok[0] = false; }
      //if (z[1] > rp.z() || z[1] < fp.z() ) 
      if (fabs(z[1]/rp.z())>1.05 ) { ok[1] = false; }
    }
    if ((! ok[0]) && (! ok[1])) {
      if(m_debug>4&&(ambig!=0)&&fabs(z[1]/rp.z())>1.05)std::cout<< " z[1] bad " << std::endl;
      if(m_debug>4&&(ambig!=0)&&fabs(z[0]/rp.z())>1.05)std::cout<< " z[0] bad " << std::endl;
      //hitUse.position(tmp);
      if(m_debug>4) {
    std::cout<< " z[1] bad " << "rpz " << rp.z() << " fpz " << fp.z() 
      << "z0  " << z[0] << " z1  " << z[1] << std::endl;
    std::cout<< " !ok[0] && !ok[1]  return -2" <<std::endl;
      }
      //return -2;
      if (ambig==-1) return_ok[ambig+1]=0;
      else           return_ok[ambig]=0;
      continue;
    }
    if (( ok[0]) && ( ok[1])) {
      //      cout<<" two z is ok  ..........?" <<endl;
    }
    unsigned best = 0;
    if (ok[1]) best = 1;
    HepVector3D p[2];
    p[0] = x + l[0] * v;
    p[1] = x + l[1] * v;
    double cosdPhi = - center.dot((p[best] - center).unit()) / center.mag();
    double dPhi;
    if(fabs(cosdPhi)<=1.0) {
      dPhi = acos(cosdPhi);
    } else if (cosdPhi>1.0) {
      dPhi = 0.0;
    } else {
      dPhi = pi;    
    }
    double stemp=r_temp*dPhi;
    if( m_debug >1) cout<<" ambig : "<<ambig<<" z: "<<z[best]<<"  stemp:  "<<stemp<<endl;
    double delta_temp=0;
    if(m_drawTuple && m_useMcInfo) delta_temp =z[best]-m_ztruth[digiId];
    if( m_debug >3) cout<<"deltatemp:   "<<delta_temp<<endl;
    // vec_delta_z.push_back(delta_temp);                      wrong   cant push_back in circulate
    //vec_delta_z[digiId]=delta_temp;
    //  delta_z=delta_temp;
    //  cout<<" vec_delta in position:  "<<delta_z<<endl;
 
    // fill z_Cacu
    if(ambig==1){
      z_Calcu_Left=z[best];
      l_Calcu_Left=stemp;
      //cout<<"ambig:  "<<ambig<<" z0 : "<<z[0]<<"  z1:  "<<z[1]<<" ztruth:  "<<m_ztruth[digiId]<<endl;
      if( m_debug >1 && m_drawTuple &&m_useMcInfo)  cout<<"ambig:  "<<ambig<<" zbest : "<<z[best]<<" ztruth:  "<<m_ztruth[digiId]<<endl;
      if( m_debug >1 && m_drawTuple &&m_useMcInfo)  cout<<"ambig:  "<<ambig<<" lbest : "<<stemp<<" ltruth:  "<<m_ltruth[digiId]<<endl;
      if( m_useMcInfo && m_drawTuple && m_postruth[digiId]==1) { z_Calcu=z[best];  l_Calcu=stemp; delta_z= delta_temp; delta_l =l_Calcu-m_ltruth[digiId];}
    }
    if(ambig==-1){
      z_Calcu_Right=z[best];
      l_Calcu_Right=stemp;
      //cout<<"ambig:  "<<ambig<<" z0 : "<<z[0]<<"  z1:  "<<z[1]<<" ztruth:  "<<m_ztruth[digiId]<<endl;
      if( m_debug >1 && m_drawTuple &&m_useMcInfo) cout<<"ambig:  "<<ambig<<" zbest : "<<z[best]<<" ztruth:  "<<m_ztruth[digiId]<<endl;
      if( m_debug >1 && m_drawTuple &&m_useMcInfo) cout<<"ambig:  "<<ambig<<" lbest : "<<stemp<<" ltruth:  "<<m_ltruth[digiId]<<endl;
      if( m_useMcInfo && m_drawTuple && m_postruth[digiId]==0) { z_Calcu=z[best];  l_Calcu=stemp; delta_z= delta_temp; delta_l =l_Calcu-m_ltruth[digiId];}
    }
 
 
    ////...Cal. xy position of crossing points...
 
    //if(m_debug>0){
    //  std::cout<<__FILE__<<" "<<__LINE__<< " p0 "<<p[0].x()<<" "<<p[0].y()<< std::endl;
    //  std::cout<<__FILE__<<" "<<__LINE__<< " p1 "<<p[1].x()<<" "<<p[1].y()<< std::endl;
    //  std::cout<<__FILE__<<" "<<__LINE__<< " c "<<center.x()<<" "<<center.y()<<" "<< std::endl;
    //  std::cout<<__FILE__<<" "<<__LINE__<< " p0 centerx*y "<<center.x() * p[0].y()<<" centery*x"<<center.y() * p[0].x()<< std::endl;
    //  std::cout<<__FILE__<<" "<<__LINE__<< " p1 centerx*y "<<center.x() * p[1].y()<<" centery*x"<<center.y() * p[1].x()<< std::endl;
    //}
 
 
    //...Which one is the best?... Study needed...
    //unsigned best = 0;
    //if (ok[1]) best = 1;
    //std::cout<<"in MdcSegInfoSterO  Zbelle "<<z[best]<<std::endl;//yzhang m_debug
 
    /*
    //...Cal. arc length...
    double cosdPhi = - center.dot((p[best] - center).unit()) / center.mag();
    double dPhi;
    if(fabs(cosdPhi)<=1.0) {
    dPhi = acos(cosdPhi);
    } else if (cosdPhi>1.0) {
    dPhi = 0.0;
    } else {
    dPhi = pi;  
    }
 
    //...Finish...
    tmp.setX(abs(r * dPhi));
    tmp.setY(z[best]);
    //hitUse.position(tmp);
 
    //z
    span->y[hitFit] = z[best];
    //if (ok[0]) span->y[hitFit] = p[0].mag();//r
    //else if(ok[1]) span->y[hitFit] = p[1].mag();
    //else span->y[hitFit] = tmp.mag();
    span->x[hitFit] = abs(r * dPhi);
    if (hitUse.ambig()<0) driftdist *= -1.;
    span->sigma[hitFit] = h.sigma(driftdist,hitUse.ambig());
 
    if (m_debug >0){
    std::cout<<"("<<h.layernumber()<<","<<h.wirenumber()<<") s "<<span->x[hitFit]<<" z "<<span->y[hitFit]<<std::endl;//yzhang m_debug
    }
    return 1;    
    */
  }
  //  cout<<"ooooooooooooooooooooooooooooooooooooooooo"<<endl;
  if(return_d[0]==0&&return_d[1]==0) return -1;
  if(return_ok[0]==0&&return_ok[1]==0) return -2;
  return 1;
}
void HoughValidUpdate::AmbigChoose(vector< vector< vector<double> > >  point,int n_use,vector<int>& ambigList,double&  tanl, double& z0){
  //connect two points 
  int n =n_use; 
  int ambig_list[4];
  //  TF1 *fpol1 =new TF1("fpol1","pol1",7,17);
  double Chis_Least;
  //TGraph *tgr[4];
  double Chis[4];
  int ambig_combine=0;
  // three points chis
  double chi_k[4];
  double chi_b[4];
  double first_x[3];
  double first_y[3];
  first_x[0]=0;
  first_y[0]=0;
  vector< vector <int> >  select(4,vector<int>() );
  for(int icombine1=0;icombine1<2;icombine1++){
    for(int icombine2=0;icombine2<2;icombine2++){
      int combine=icombine1*2+icombine2;
      first_x[1]=point[icombine1][0][n-1];
      first_x[2]=point[icombine2][0][n-2];
      first_y[1]=point[icombine1][1][n-1];
      first_y[2]=point[icombine2][1][n-2];
      double k,b;
      double chi2=0;
      LeastLine(3,first_x,first_y,k,b,chi2);
      // TGraph *gr_first=new TGraph(3,first_x,first_y);
      //gr_first->Fit("fpol1","QN");
      //delete gr_first;
      //double k=fpol1->GetParameter(1);
      //double b=fpol1->GetParameter(0);
      //cout<<"K: "<<k<<" B: "<<b<<endl;
 
      //      sprintf(hname,"h%d",combine);
      //      h[combine]=new TH2D("hname","hname",100,7,17,100,-5,35);
      //      double k=(point[icombine1][1][n-1]-point[icombine2][1][n-2])/(point[icombine1][0][n-1]-point[icombine2][0][n-2]);   //left-left
      //      double b=point[icombine1][1][n-1]-k*point[icombine1][0][n-1];
      //      cout<<"k: "<<k<<"b: "<<b<<endl;
 
      for(int iHit=0;iHit<n-2;iHit++){
    double dist_to_line[2];
    for(int j=0;j<2;j++){
      double x=point[j][0][iHit];
      double y=point[j][1][iHit];
      dist_to_line[j]=( abs(k*x-y+b) ) / sqrt( k*k +1);
    }
    if(dist_to_line[0]>dist_to_line[1])  select[combine].push_back(1);
    else select[combine].push_back(0);
    //      cout<<select[combine][iHit]<<endl;
      }
      select[combine].push_back(icombine2);       //left -left 
      select[combine].push_back(icombine1);
 
      //least 2*  fit
      //add (0,0)
      //      vector<double> l; 
      //      vector<double> z; 
      double *l_new=new double[n+1];
      double *z_new=new double[n+1];
      for(int i=0;i<n;i++){
    int ambig=select[combine][i];
    l_new[i]=point[ambig][0][i];
    z_new[i]=point[ambig][1][i];
      }
      l_new[n]=0;
      z_new[n]=0;
      //for(int i=0;i<n;i++){
      //  int ambig=select[combine][i];
      //  cout<<"ambig : "<<ambig<<endl;
      //  l.push_back(point[ambig][0][i]);
      //  z.push_back(point[ambig][1][i]);
      //  cout<<"hit: "<<i<<" :("<<l[i]<<","<<z[i]<<")"<<endl;
      //}
      //l.push_back(0);
      //z.push_back(0);
 
      double k_least,b_least;
      double chi2_least=0;
      LeastLine(n+1,l_new,z_new,k_least,b_least,chi2_least);
      Chis[combine]=chi2_least;
      chi_k[combine]=k_least;
      chi_b[combine]=b_least;
 
      //      tgr[combine]=new TGraph(n+1,l_new,z_new);                             
      //      tgr[combine]->Fit("fpol1","QN");
      delete []l_new;
      delete []z_new;
 
 
      //  TH2D *h1=new TH2D("h1","h1",100,7,17,100,-5,35);
      //for(int i =0 ;i<n+1; i++){
      //  h[combine]->Fill(l[i],z[i]);
      //  //h->Draw("SAME");
      //}
      //      h[combine]->Fit("fpol1");
      //Chis[combine]=fpol1->GetChisquare();
      //chi_k[combine]=fpol1->GetParameter(1);
      //chi_b[combine]=fpol1->GetParameter(0);
      //      cout<<" chis: "<<Chis[combine]<<endl;
      //delete tgr[combine];
      //      delete h[combine];
    }     //icombine1
  }     //icombine2
  //    for(int i =0;i<4;i++){
  //      delete h[i];
  //    }
  //  delete fpol1;
  Chis_Least=Chis[0];
  for(int i=0;i<4;i++){
    if(Chis_Least>=Chis[i]) {Chis_Least=Chis[i]; ambig_combine=i;/* cout<<"least i: "<< i<<endl;*/}
  }
  for(int iHit=0;iHit<n;iHit++){
    ambigList.push_back(select[ambig_combine][iHit]);
    //  cout<<"ambig_combine: "<<ambig_combine<<endl;
    //  cout<<" iHit: "<<select[ambig_combine][iHit]<<endl;
    //  cout<<" iHit: "<<ambigList[iHit]<<endl;
  }
 
  tanl=chi_k[ambig_combine];
  z0=chi_b[ambig_combine];
  //  cout<<"Chi least: "<<Chis_Least<<" ambig: "<<ambig_combine<<endl;
  // cout<<"ambig_combine: "<<ambig_list[ambig_combine]<<endl;
}
void HoughValidUpdate::Combine(TH2D *h1,int widthX,int widthY,vector< vector < vector<int> > > vec_selectNum,M1 &peak_hit_list,M2 &peak_hit_num,int &peak_track,vector<int> peakList[]){
  int m=0;
  int n=0;
  int addnum=999;
  int npeak1=peak_track;
  vector<bool> peaktrack(npeak1,true);
  while(addnum!=0)
  {
    //  cout<<"Ntrack: "<<n<<endl;
    addnum=0;
    //  int peakXtemp=peakList[0][0]-1;
    //  int peakYtemp=peakList[1][0]-1;
    //  cout<<"vec_selectpeak: ("<<peakXtemp<<","<<peakYtemp<<"): "<<vec_selectNum[peakXtemp][peakYtemp].size()<<endl;
    //  for(int i =0;i<vec_selectNum[peakXtemp][peakYtemp].size();i++){cout<<" iN:vec: "<<vec_selectNum[peakXtemp][peakYtemp][i]<<endl;}
 
    int peakX=peakList[0][n];
    int peakY=peakList[1][n];
    for (int px=peakX-widthX; px<=peakX+widthX; px++) {
      for (int py=peakY-widthY; py<=peakY+widthY; py++) {
    int ax;
    if (px<0) { ax=px+binX; }
    else if (px>=binX) { ax=px-binX; }
    else { ax=px; }
    if ( (ax!=peakX || py!=peakY) && py>=0 && py<binY)  Combine_two_cell(h1,peakX,peakY,ax,py,vec_selectNum,m,peak_hit_list,peak_hit_num);
      }
    }
    //  for(int i=0;i<peak_hit_list[m].size();i++){
    //      cout<<"COMBINE: "<<peak_hit_list[m][i]<<endl;
    //  }
 
    for(int i=n+1;i<npeak1;i++){
      if(peaktrack[i]==false) continue;
      int peaktheta=peakList[0][i];
      int peakrho=peakList[1][i];
      int peakhitNum=peakList[2][i];
      int count_same_hit=0;
      for(int j=0;j<peakhitNum;j++){
    for(int k=0;k<peak_hit_num[m];k++){
      if(vec_selectNum[peaktheta][peakrho][j]==peak_hit_list[m][k]){
        count_same_hit++;
      }
    }
      }
      if(m_debug>1) cout<<" pro:  "<<"peak: "<<i<<"  "<<(double)count_same_hit/peakhitNum<<endl;
      double f_hit=m_hit_pro;
      if(count_same_hit>f_hit*peakhitNum){
    peaktrack[i]=false;
    //      cout<<" track_candi: "<<i<<" is false"<<endl;
      }
    }
    for(int i=n+1;i<npeak1;i++){
      if(peaktrack[i]==true){
    addnum=i-n;
    break;
      }
    }
    if(addnum!=0) m++; 
    n=n+addnum;
  }
  int ntrack=0;
  for(int i=0;i<npeak1;i++){
    //cout<<"track"<<i<<":  "<<"("<<peakList[0][i]<<","<<peakList[1][i]<<","<<peakList[2][i]<<")"<<"  "<<"truth:  "<<peaktrack[i]<<endl;
    if(peaktrack[i]==true){
      ntrack++;
    }
  }
  peak_track=ntrack;
}
void HoughValidUpdate::Combine_two_cell(TH2D *h1,int xPeakCell,int yPeakCell,int xAround,int yAround,vector< vector < vector<int> > > vec_selectNum,int m,M1 &peak_hit_list,M2 &peak_hit_num){
  int size_of_around=vec_selectNum[xAround][yAround].size();
  if(size_of_around==0) return;
  // cout<<"size of around : "<<size_of_around<<endl;
  //for(int iaround=0;iaround<size_of_around;iaround++){
  //  cout<<"around: "<<vec_selectNum[xAround][yAround][iaround]<<endl;
  //}
  int size_of_peak=vec_selectNum[xPeakCell][yPeakCell].size();
  //  cout<<"size of peak: "<<size_of_peak<<endl;
  //  for(int ipeak=0;ipeak<size_of_peak;ipeak++){
  //    cout<<"peak: "<<vec_selectNum[xPeakCell][yPeakCell][ipeak]<<endl;
  //  }
  if(peak_hit_list[m].size()==0){
    for(int inum=0;inum<size_of_peak;inum++){
      peak_hit_list[m][inum]=vec_selectNum[xPeakCell][yPeakCell][inum];
    }
  }
  vector<int> vec_more_select;
  for (int i =0;i<size_of_around;i++){
    int same_vec_with_hitcombin=0;
    for(int j= 0;j<peak_hit_list[m].size();j++){
      if(vec_selectNum[xAround][yAround][i]==peak_hit_list[m][j]){
    same_vec_with_hitcombin=1;
    break;
      }
    }
    if(same_vec_with_hitcombin==0) {
      //      cout<<" peak_hit_list push : "<<vec_selectNum[xAround][yAround][i]<<endl;
      vec_more_select.push_back(vec_selectNum[xAround][yAround][i]);
    }
  }
  int peak_hit_list_size=peak_hit_list[m].size();
  for(int i=0;i<vec_more_select.size();i++ ){
    peak_hit_list[m][i+peak_hit_list_size]=vec_more_select[i];  
  }
  int peak_hit_list_size_new=peak_hit_list[m].size();
  peak_hit_num[m]=peak_hit_list_size_new;
  //  cout<<"peak_combine: "<<peak_hit_num[m]<<endl;
  for(int i= 0;i<peak_hit_list_size_new;i++){
    //  cout<<" peak_hit_list: "<<peak_hit_list[m][i]<<endl;
  }
 
 
 
  //  for(int i =0;i<size_of_around;i++){
  //    int count_same_with_peak=0;
  //    for(int j =0;j<vec_selectNum[xPeakCell][yPeakCell].size();j++){
  //      if(vec_selectNum[xAround][yAround][i]==vec_selectNum[xPeakCell][yPeakCell][j]) {
  //        count_same_with_peak=1;
  //        break;
  //      }
  //    }
  //    if(count_same_with_peak==0) { 
  //      // cout<<"  push :  "<<vec_selectNum[xAround][yAround][i]<<endl;
  //      vec_selectNum[xPeakCell][yPeakCell].push_back(vec_selectNum[xAround][yAround][i]);
  //    } 
  //  }
  //
  //  int size_of_peak_update=vec_selectNum[xPeakCell][yPeakCell].size();
}
//bool HoughValidUpdate::operator<(const HoughValidUpdate::Mytrack& a,const HoughValidUpdate::Mytrack& b){
//  return a.pt>b.pt;
//}
bool compare(const HoughValidUpdate::Mytrack& a,const HoughValidUpdate::Mytrack& b){
  return abs(a.pt)>abs(b.pt);
}
int HoughValidUpdate::LeastLine(int nhit,double x[],double y[],double &k,double &b,double& chi2){
  double x_sum=0;
  double y_sum=0;
  double x2_sum=0;
  double y2_sum=0;
  double xy_sum=0;
  for(int i=0;i<nhit;i++){
    x_sum=x_sum+x[i];
    y_sum=y_sum+y[i];
    x2_sum=x2_sum+x[i]*x[i];
    y2_sum=y2_sum+y[i]*y[i];
    xy_sum=xy_sum+x[i]*y[i];
  }
  b=(x2_sum*y_sum-xy_sum*x_sum)/(nhit*x2_sum-x_sum*x_sum);
  k=(nhit*xy_sum-x_sum*y_sum)/(nhit*x2_sum-x_sum*x_sum);
 
  double yE[3];
  for(int i=0;i<nhit;i++){
    yE[i]=k*x[i]+b;
    double chi2_temp;
    if(yE[i]!=0) chi2_temp=(y[i]-yE[i])*(y[i]-yE[i])/yE[i]*yE[i];
    else chi2_temp=0;
    chi2+=chi2_temp;
  }
 
  return 1;
}