HoughMap.cxx

Go to the documentation of this file.

#include "MdcHoughFinder/HoughMap.h"
#include "MdcHoughFinder/HoughPeak.h"
#include <vector>
#include <algorithm>
#include <cmath>
 
int HoughMap::m_debug=0;
int HoughMap::m_useHalfCir=0;
int HoughMap::m_N1;
int HoughMap::m_N2;
int HoughMap::m_nPoint2D=1;
int HoughMap::m_method=1;
double HoughMap::m_nRMS=3;
 
HoughMap::HoughMap(){
    //_mapHitList=NULL;
    _houghSpace=NULL;
}
 
HoughMap::HoughMap(int charge,HoughHitList &houghHitList,int mapHit ,int ntheta,int nrho,double rhoMin,double rhoMax,int peakWidth,int peakHigh,double hitpro){
    _hitList=houghHitList;
    _mapHit=mapHit;
    _nTheta=ntheta;
    _nRho=nrho;
    _thetaMin=0;
    _thetaMax=M_PI;
    _rhoMin=rhoMin;
    _rhoMax=rhoMax;
    _peakWidth=peakWidth;
    _peakHigh=peakHigh;
    _hitpro=hitpro;
    _charge=charge;
 
    //_mapHitList = new vector<const HoughHit*>*[_nTheta];
    //_houghS=  new double*[_nTheta];
    //_houghS2=  new double*[m_N2];
    //  _houghR=  new double*[_nTheta];
    //  _houghRL=  new double*[_nTheta];
    //  _houghNL=  new double*[_nTheta];
 
    //for(int i=0; i<_nTheta; i++){ 
    //_mapHitList[i] = new vector<const HoughHit*>[_nRho];
    //  _houghS[i] = new double[_nRho];
    //  _houghR[i] = new double[_nRho];
    //  _houghRL[i] = new double[_nRho];
    //  _houghNL[i] = new double[_nRho];
    //  for(int j=0; j<_nRho; j++){ 
    //    _houghS[i][j]=0;
    //    _houghR[i][j]=0;
    //  }
    //}
    //  for(int i=0; i<m_N2; i++){ 
    //    _houghS2[i] = new double[m_N2];
    //    for(int j=0; j<m_N2; j++){ 
    //      _houghS2[i][j]=0;
    //    }
    //  }
 
    if(_charge==-1) _houghSpace = new TH2D("houghspace","houghspace",_nTheta,_thetaMin,_thetaMax,_nRho,_rhoMin,_rhoMax);
    if(_charge==1 ) _houghSpace = new TH2D("houghspace2","houghspace2",_nTheta,_thetaMin,_thetaMax,_nRho,_rhoMin,_rhoMax);
    //if(_charge==-1) _houghSpace2 = new TH2D("houghSpace","houghSpace",_nTheta,_thetaMin,_thetaMax,_nRho,_rhoMin,_rhoMax);
    //if(_charge==1 ) _houghSpace2 = new TH2D("houghSpace2","houghSpace2",_nTheta,_thetaMin,_thetaMax,_nRho,_rhoMin,_rhoMax);
    //  _houghR= new TH2D("houghR","houghR",_nTheta,_thetaMin,_thetaMax,_nRho,_rhoMin,_rhoMax);
 
    doMap();
}
HoughMap::HoughMap(const HoughMap& other){
    _mapHit  =other._mapHit,
             _nTheta  =other._nTheta,
             _nRho    =other._nRho,
             _thetaMin=other._thetaMin,
             _thetaMax=other._thetaMax,
             _rhoMin  =other._rhoMin,
             _rhoMax  =other._rhoMax,
             _hitList =other._hitList,
             _houghPeakList=other._houghPeakList,
             _houghTrackList=other._houghTrackList,
             _houghSpace = other._houghSpace,
             _charge = other._charge;
    //_houghR= other._houghR;
 
    //_mapHitList = new vector<const HoughHit*>*[_nTheta];
    //_houghNL= new double*[_nTheta];
    //_houghRL= new double*[_nTheta];
    //_houghS= new double*[_nTheta];
    //for(int i=0; i<_nTheta; i++){ 
    //_mapHitList[i] = new vector<const HoughHit*>[_nRho];
    //  _houghNL[i] = new double[_nRho];
    //  _houghRL[i] = new double[_nRho];
    //_houghS[i] = new double[_nRho];
    //}
 
    //for(int i=0; i<_nTheta; i++){ 
    //  for(int j=0; j<_nRho; j++){ 
    //_mapHitList[i][j]=other._mapHitList[i][j];
    //    _houghNL[i][j]=other._houghNL[i][j];
    //    _houghRL[i][j]=other._houghRL[i][j];
    //    _houghS[i][j]=other._houghS[i][j];
    //  }
    // }
}
 
void HoughMap::doMap(){
    if(m_method==1)buildMap();
    if(m_method==2)findPeaks();
    //{
        //buildHoughMap();
        //findHoughPeaks();
    //}
    //mapDev();
    //cald_layer();
    //select_slant();
    //gravity();
    //findPeaks();
    //if(m_debug>0)  {cout<<" before sort "<<endl;  printPeak();}
     //{cout<<" before sort "<<endl;  printPeak();} 
    sortPeaks();
    //{cout<<" after sort "<<endl;  printPeak();} 
    hitFinding();
    trackFinder();
    if(m_debug>0)  { cout<<" after sort "<<endl;  printPeak();}
    // candiTrack();
    if(m_debug>0) printTrack();
}
 
void HoughMap::clearMap(){
 
    //  for(int i=0; i<_nTheta; i++){ delete []_mapHitList[i];delete []_houghNL; delete []_houghRL;delete []_houghS;}
    //  for(int i=0; i<_nTheta; i++){ delete []_mapHitList[i]; }
    // delete []_mapHitList;
    delete _houghSpace;
    //delete _houghSpace2;
    //  for(int i=0; i<_nTheta; i++){ delete []_houghS[i];}
    //  delete []_houghS;
    //  delete _houghR;
    //  for(int i=0; i<m_N2; i++)   { delete []_houghS2[i];}
    //  delete []_houghS2;
}
 
HoughMap::~HoughMap(){
    clearMap();
}
void HoughMap::buildMap(){
    if(m_debug>0) cout<<"in build map "<<endl;
    //cout<<m_slantCut<<"  "<<m_mapcut<<endl;
    //loop over all hits, fill histogram of HoughRec space
    //std::vector<HoughHit>::const_iterator iter = _hitList.getList().begin();
    std::vector<HoughHit>::iterator iter = _hitList.getList().begin();
    for(; iter!= _hitList.getList().end(); iter++){
        HoughHit *hit = &(*iter);
        // --- add CGEM X cluster to map
        if(hit->getDetectorType() == CGEM &&  (hit->getCluster())->getflag() !=0 ) continue;
        if( _mapHit==1 && hit->getSlayerType()!=0  ) continue;
        if( hit->driftTime()>1000 && hit->driftTime()<0 ) continue;              //to big hit ,maybe error hit
        double drift_CF = hit->getR();
        hit->makeCir(2*_nTheta,0,2*M_PI,drift_CF);   //N2
        //hit->print();
        for( int ni=0;ni<2*m_N1;ni++){   //N1
            double binwidth = M_PI/_nTheta;
            double binhigh = (_rhoMax-_rhoMin)/_nRho;
            double theta = hit->getCir(ni).getTheta();
            double rho = hit->getCir(ni).getRho();
            //cout<<theta<<"  "<<rho<<endl;
            if( fabs(rho) >=_rhoMax ) continue;
            double slantOfLine = hit->getCir(ni).getSlant(); 
            int chargeOfHitOnCir = (slantOfLine/fabs(slantOfLine)) * (rho/fabs(rho));
            if(chargeOfHitOnCir == _charge) continue;  
            //if( fabs(slantOfLine)<0.03 ) continue;      //FIXME
            _houghSpace->Fill(theta,rho);
            //    int rhobinNum = (int)( (rho-_rhoMin)/((_rhoMax-_rhoMin)/m_N1) );
            //    int thetabinNum = (int)( (theta-_thetaMin)/((_thetaMax-_thetaMin)/m_N1) );
            //    _mapHitList[thetabinNum][rhobinNum].push_back(hit) ;
        }
        //break;  //only first line
    }
    //initialize
    //  double hist[1000][1000] ={0};
    //  int max =0 ;
    double aver(0),sigma(0);
    mapDev(_houghSpace,aver,sigma);
    //cout<<"aver, sigma, cut: "<<aver<<" "<<sigma<<" "<<aver+ m_mapcut*sigma<<endl;
    //double a[7];
    //_houghSpace->GetStats(a);
    //for(int ai =0;ai<7;ai++)cout<<a[ai]<<"  ";
    //cout<<endl;
    for (int iTheta=0; iTheta<_nTheta; iTheta++) {
        for (int iRho=0; iRho<_nRho; iRho++) {
            int  z=_houghSpace->GetBinContent(iTheta+1,iRho+1);
            double thetabin = _houghSpace->GetXaxis()->GetBinCenter(iTheta+1);
            double rhobin = _houghSpace->GetYaxis()->GetBinCenter(iRho+1);
            double pt = (1/rhobin)/333.567;
            int    MINCOUNT = 10;   //num at least consider exist a track
            if(0.06<=pt&&pt<0.07 ) MINCOUNT = 6;
            if(pt<0.06) MINCOUNT = 5;
            double minCount_Cut=( aver+4*sigma >MINCOUNT)? (aver+4*sigma):MINCOUNT;  //  4 temp par
            //    _houghS[iTheta][iRho]=z;
            //if( z>=MINCOUNT) loopPeak(thetabin,rhobin,iTheta,iRho,hist);
            //if( z>=MINCOUNT) loopPeak(thetabin,rhobin,iTheta,iRho);
            //if( z>=minCount_Cut)cout<<"cut "<<minCount_Cut<<"  z "<<z<<endl;
            //if( z>=minCount_Cut)cout<<minCount_Cut<<endl;
            //if( z>=minCount_Cut)cout<<setw(5)<<iTheta+1<<setw(5)<<iRho+1<<setw(5)<<z<<setw(12)<<thetabin<<setw(12)<<rhobin<<endl;
            if( z>=minCount_Cut) loopPeak(thetabin,rhobin,iTheta,iRho);
            //int binx,biny,binz;
            //_houghSpace->GetMaximumBin(binx,biny,binz);
            //    if( max <z ) max = z ;
        }
    }
    // MAX = max;
    //  if(m_debug>0) printMapHit();
 
}
void HoughMap::buildHoughMap(int x_bin, double x_min, double x_max, int y_bin, double y_min, double y_max, int nPoint, int peak)
{
    stringstream ssname;
    ssname<<peak;
    string sname = ssname.str();
    const char * name = sname.c_str() ;
    _houghMap = new TH2D(name,name, x_bin, x_min, x_max, y_bin, y_min, y_max);
    std::vector<HoughHit>::iterator iter = _hitList.getList().begin();
    for(; iter!= _hitList.getList().end(); iter++){
        if((iter)->getSlayerType()!=0) continue;
        if( (iter)->driftTime()>1000 && (iter)->driftTime()<0 ) continue;
        (iter)->buildMap(x_bin, x_min, x_max, y_bin, y_min, y_max,m_nPoint2D,_charge);
        _houghMap->Add((iter)->getHitMap());
    }
}
 
void HoughMap::clearHoughMap()
{
    std::vector<HoughHit>::iterator iter = _hitList.getList().begin();
    for(; iter!= _hitList.getList().end(); iter++){
        if((iter)->getHitMap() != NULL)(iter)->clearMap();
    }
    if(_houghMap != NULL)delete _houghMap;
    _houghMap = NULL;
}
 
void HoughMap::findHoughPeaks(vector<HoughPeak>& vecPeak)
{
    int binx(0),biny(0),binz(0);
    double theta(0) , rho(0);
    //double aver(0) , sigma(0) ,cut(0);
    int height(0);
    int peak = vecPeak.size();
    _houghMap->GetMaximumBin(binx,biny,binz);
    height = _houghMap->GetBinContent(binx,biny);
    double cut = mapDev(_houghMap,m_nRMS);
    do{
    //while(height>cut){
        peak++;
        _houghMap->GetMaximumBin(binx,biny,binz);
        theta = _houghMap->GetXaxis()->GetBinCenter(binx);
        rho = _houghMap->GetYaxis()->GetBinCenter(biny);
        height = _houghMap->GetBinContent(binx,biny);
        //cout<<"peak"<<setw(5)<<peak<<setw(5)<<binx<<setw(5)<<biny<<setw(12)<<theta<<setw(12)<<rho<<setw(12)<<1.0/333.567/rho<<"  "<<height;
        //cout<<endl;
        //double a[7];
        //_houghMap->GetStats(a);
        //for(int ai =0;ai<7;ai++)cout<<a[ai]<<"  ";
        //cout<<endl;
        //loopPeak(theta,rho,binx,biny);
        vecPeak.push_back( HoughPeak(height,binx,biny,theta,rho,true,peak,_charge) );//store peak
        int hot(0);
        std::vector<HoughHit>::iterator iter = _hitList.getList().begin();
        for(; iter!= _hitList.getList().end(); iter++){
            if((iter)->getHitMap() == NULL)continue;
            if((iter)->getHitMap()->GetBinContent(binx,biny)>0){
                _houghMap->Add((iter)->getHitMap(),-1);
                //const char * title = (iter)->getHitMap()->GetTitle();
                //cout<<"remove "<<title<<endl;
                hot++;
                (iter)->clearMap();
            }
        }
        //cout<<"remove "<<hot<<" hits"<<endl;
        _houghMap->GetMaximumBin(binx,biny,binz);
        height = _houghMap->GetBinContent(binx,biny);
        cut = mapDev(_houghMap,m_nRMS);
        //cout<<"aver="<<aver<<" , RMS="<<sigma<<"  ,cut="<<cut<<"  , peak height="<<height<<endl;
        //cout<<endl;
        if(peak>_hitList.getHitNumA(0))break;
        //if(aver<_houghMap->GetEntries()/(_nRho*_nTheta))break;
    }
    while(height>cut);
 
}
 
void HoughMap::findPeaks()
{
    int ext[2] = {0};
    int peak = 0;
    int nPoint = m_nPoint2D;
    int x_bin = _nTheta;
    int y_bin = _nRho;
    double x_min = _thetaMin;
    double x_max = _thetaMax;
    double y_min = _rhoMin;
    double y_max = _rhoMax;
    double dx = (x_max - x_min)/x_bin;
    double dy = (y_max - y_min)/y_bin;
 
    vector<HoughPeak> vecPeak_tmp;
    buildHoughMap(x_bin, x_min, x_max, y_bin, y_min, y_max,nPoint,peak);
    findHoughPeaks(vecPeak_tmp);
    clearHoughMap();
 
    std::vector<HoughPeak>::iterator iter = vecPeak_tmp.begin();
    for(;iter != vecPeak_tmp.end();iter++){
        ext[0] = 2;
        ext[1] = 2;
        peak = iter->getPeakNum();
        nPoint = m_nPoint2D;
        x_bin = 10*(2*ext[0]+1);
        y_bin = 10*(2*ext[1]+1);
        x_min = iter->getTheta() - (0.5+ext[0])*dx;
        x_max = iter->getTheta() + (0.5+ext[0])*dx;
        y_min = iter->getRho()   - (0.5+ext[1])*dy;
        y_max = iter->getRho()   + (0.5+ext[1])*dy;
        //dx = (x_max - x_min)/x_bin;
        //dy = (y_max - y_min)/y_bin;
        //cout<<" ------------------------------------------------ ";
        //cout<<peak;
        //cout<<" ------------------------------------------------ ";
        //cout<<endl;
        buildHoughMap(x_bin, x_min, x_max, y_bin, y_min, y_max,nPoint,peak);
        findHoughPeaks(_houghPeakList);
        clearHoughMap();
    }
}
/*
   void HoughMap::printMapHit(){
   double aver(0),sigma(0);
   mapDev(_houghSpace,aver,sigma);
//cout<<"AVER SIGMA "<<aver<<" "<<sigma<<endl;
//cout<<"print Map Hit : "<<_charge<<endl;
for(int i =0;i<m_N1;i++){
for(int j =0;j<m_N1;j++){
if( _mapHitList[i][j].size()>=5 ) cout<<"map :("<<i<<","<<j<<") :"<<_mapHitList[i][j].size()<<endl;
else continue;
for(int k =0;k<_mapHitList[i][j].size();k++){
cout<<"Hit: ("<<_mapHitList[i][j][k]->getLayerId()<<","<<_mapHitList[i][j][k]->getWireId()<<")"<<endl;
}
}
}
}
*/
 
void HoughMap::loopPeak(double thetabin,double rhobin,int iTheta,int iRho){
    //int binx,biny,binz;
    int ntheta=m_N2;
    int nrho=m_N2;
    double theta_left = thetabin-(5/2.)*(M_PI/_nTheta);
    double theta_right= thetabin+(5/2.)*(M_PI/_nTheta);
    double rho_down=  rhobin-(5/2.)*((2*_rhoMax)/_nRho);
    double rho_up =   rhobin+(5/2.)*((2*_rhoMax)/_nRho);
    cout<<"("<<theta_left<<", "<<theta_right<<")"<<"    ("<<rho_down<<", "<<rho_up<<")"<<endl;
    // cout<<"binx biny "<<binx<<" "<<biny<<endl;
    vector< vector<int> > vec_hist(nrho,vector<int>(ntheta,0) );
    for(int itheta =0;itheta<ntheta;itheta++){
        for(int irho=0;irho<nrho;irho++){
            vec_hist[itheta][irho]=0;
        }
    }
 
    std::vector<HoughHit>::iterator iter = _hitList.getList().begin();
    for(; iter!= _hitList.getList().end(); iter++){
        HoughHit *hit = &(*iter);
        if(hit->getDetectorType() == CGEM &&  (hit->getCluster())->getflag() !=0 ) continue;
        if( _mapHit==1 && hit->getSlayerType()!=0 ) continue;
        if( hit->driftTime()>1000 ) continue;              //to big hit ,maybe error hit
        double drift_CF = hit->getR();
        //cout<<hit->getLayerId()<<endl;
 
        // from 0 to M_PI
        hit->makeCir(m_N2,theta_left,theta_right,drift_CF);   //N2
        for( int ni=0;ni<m_N2;ni++){
            double theta = hit->getCir(ni).getTheta();
            double rho = hit->getCir(ni).getRho();
            double slantOfLine = hit->getCir(ni).getSlant(); 
            int chargeOfHitOnCir = (slantOfLine/fabs(slantOfLine)) * (rho/fabs(rho));
            if(chargeOfHitOnCir == _charge ) continue;  
            //if( fabs(slantOfLine)<0.03 ) continue;  
            //
            //cout<<"theta_left theta_right theta:"<<theta_left<<"  "<<theta_right<<"  "<<theta<<endl;
            //cout<<"rho_down   rho_up      rho  :"<<rho_down<<"  "<<rho_up<<"  "<<rho<<endl;
            if( theta<=theta_left|| theta>=theta_right) continue;
            if( rho<=rho_down || rho>=rho_up ) continue;
            int rhobinNum = (int)( (rho-rho_down)/((rho_up-rho_down)/m_N2) );
            int thetabinNum = (int)( (theta-theta_left)/((theta_right-theta_left)/m_N2) );
            vec_hist[thetabinNum][rhobinNum]++;
            //cout<<"vec_hist"<<vec_hist[thetabinNum][rhobinNum]<<endl;
            //hist[thetabinNum+iTheta*ntheta][rhobinNum+iRho*nrho]++;
        }
 
        // from M_PI to 2*M_PI
        hit->makeCir(m_N2,theta_left+M_PI,theta_right+M_PI,drift_CF);   //N2
        for( int ni=0;ni<m_N2;ni++){
            double theta = hit->getCir(ni).getTheta();
            double rho = hit->getCir(ni).getRho();
            double slantOfLine = hit->getCir(ni).getSlant(); 
            int chargeOfHitOnCir = (slantOfLine/fabs(slantOfLine)) * (rho/fabs(rho));
            if(chargeOfHitOnCir == _charge ) continue;  
            //if( fabs(slantOfLine)<0.03 ) continue;  
            //
            if( theta<=theta_left|| theta>=theta_right) continue;
            if( rho<=rho_down || rho>=rho_up ) continue;
            int rhobinNum = (int)( (rho-rho_down)/((rho_up-rho_down)/m_N2) );
            int thetabinNum = (int)( (theta-theta_left)/((theta_right-theta_left)/m_N2) );
            vec_hist[thetabinNum][rhobinNum]++;
            //cout<<"vec_hist"<<vec_hist[thetabinNum][rhobinNum]<<endl;
            //hist[thetabinNum+iTheta*ntheta][rhobinNum+iRho*nrho]++;
            //cout<<"("<<thetabinNum+iTheta*ntheta<<","<<rhobinNum+iRho*nrho<<"): "<<hist[thetabinNum+iTheta*ntheta][rhobinNum+iRho*nrho]<<endl;
        }
    }  //loop over hit
 
    findPeaks(vec_hist,theta_left,theta_right,rho_down,rho_up);
}
 
 
//find local maximum in hough space between 8 neighbor cells
void HoughMap::findPeaks(vector< vector<int> > vec_hist,double theta_left,double theta_right,double rho_down,double rho_up){
    int minCount_Cut;
    double rho_peak = 1./2.*(rho_down+rho_up);
    double pt_peak = (1./rho_peak)/333.567;
    if (0.06<=pt_peak&&pt_peak<0.07 )  minCount_Cut = 4; 
    if (pt_peak <0.06 )  minCount_Cut = 3; 
    else minCount_Cut = 5;
    double aver(0),sigma(0);
    //mapDev(vec_hist,aver,sigma);
    //int minCount_Cut=( aver+3*sigma> 5)?( aver+3*sigma):5;
    //cout<<"aver: "<<aver<<" "<<sigma<<endl;
    int _nTheta = m_N2;
    int _nRho= m_N2;
 
    // store peak property
    // 0: not a peak, 
    // 1: same hight with neighbor cells, 
    // 2: highest peak according to neighbor 8 cells
    int **hough_trans_CS_peak = new int*[_nTheta];
    for(int i=0; i<_nTheta; i++){ 
        hough_trans_CS_peak[i] = new int[_nRho];
        for(int j=0; j<_nRho; j++){ hough_trans_CS_peak[i][j]=-999;} 
    }
 
    //loop over hough space
    for (int iRho=0; iRho<_nRho; iRho++) {
        for (int iTheta=0; iTheta<_nTheta; iTheta++) {
            //int height = houghSpace->GetBinContent(iTheta+1,iRho+1);
            int height = vec_hist[iTheta][iRho];
            //cout<<"heght "<<height<<endl;
            if(height < minCount_Cut) {
                hough_trans_CS_peak[iTheta][iRho]=0; 
                continue;
            }
            //loop over around bins
            int max_around=0;            
            for (int ar=iTheta-1; ar<=iTheta+1; ar++) {
                for (int rx=iRho-1; rx<=iRho+1; rx++) {
                    int ax;
                    if (ar<0) { ax=ar+_nTheta; }
                    else if (ar>=_nTheta) { ax=ar-_nTheta; }
                    else { ax=ar; }
                    if ( (ax!=iTheta || rx!=iRho) && rx>=0 && rx<_nRho) {
                        //      int heightThisBin =houghSpace->GetBinContent(ax+1,rx+1);
                        int heightThisBin = vec_hist[ax][rx];
                        if (heightThisBin>max_around) { max_around=heightThisBin; }
                    }
                }
            }
            if (height<max_around) { hough_trans_CS_peak[iTheta][iRho]=0; }
            else if (height==max_around) { hough_trans_CS_peak[iTheta][iRho]=1; }
            else if (height>max_around) { hough_trans_CS_peak[iTheta][iRho]=2; }
        }
    }
    //  mergeNeighbor(hough_trans_CS_peak,theta_left,theta_right,rho_down,rho_up);
    //merge 
    int peakNum_Merge=0;
    for (int iRho=0; iRho<_nRho; iRho++) {
        for (int iTheta=0; iTheta<_nTheta; iTheta++) {
            if (hough_trans_CS_peak[iTheta][iRho]==1) {
                hough_trans_CS_peak[iTheta][iRho]=2;
                for (int ar=iTheta-1; ar<=iTheta+1; ar++) {
                    for (int rx=iRho-1; rx<=iRho+1; rx++) {
                        int ax;
                        if (ar<0) { ax=ar+_nTheta; }
                        else if (ar>=_nTheta) { ax=ar-_nTheta; }
                        else { ax=ar; }
                        if ( (ax!=iTheta || rx!=iRho) && rx>=0 && rx<_nRho) {
                            if (hough_trans_CS_peak[ax][rx]==1) { hough_trans_CS_peak[ax][rx]=0; }
                        }
                    }
                }
            }
            if(hough_trans_CS_peak[iTheta][iRho]==2) {
                int peak_num = _houghPeakList.size();
                //      _houghPeakList.push_back( HoughPeak(_houghSpace->GetBinContent(iTheta+1,iRho+1),iTheta,iRho,exTheta(iTheta,theta_left,theta_right,m_N2),exRho(iRho,rho_down,rho_up,m_N2),true,peak_num+1,_charge) );//store peak
                _houghPeakList.push_back( HoughPeak(vec_hist[iTheta][iRho],iTheta,iRho,exTheta(iTheta,theta_left,theta_right,m_N2),exRho(iRho,rho_down,rho_up,m_N2),true,peak_num+1,_charge) );//store peak
                //cout<<"height "<<vec_hist[iTheta][iRho]<<endl;
                peakNum_Merge++;
                cout<<setw(3)<<iTheta<<setw(3)<<iRho<<setw(5)<<vec_hist[iTheta][iRho]<<setw(12)<<theta_left<<setw(12)<<theta_right<<setw(12)<<rho_down<<setw(12)<<rho_up<<setw(12)<<exTheta(iTheta,theta_left,theta_right,m_N2)<<setw(12)<<exRho(iRho,rho_down,rho_up,m_N2)<<endl;
            }
        }
    }
 
    // end of merge
    for(int i=0; i<_nTheta; i++){ delete []hough_trans_CS_peak[i]; }
    delete []hough_trans_CS_peak;
}
/*
//merge "1" peaks, 1: same hight with neighbor peaks
int HoughMap::mergeNeighbor(int** hough_trans_CS_peak,double theta_left,double theta_right,double rho_down,double rho_up){
//  _houghPeakList.reserve(100);
int _nTheta = m_N2;
int _nRho= m_N2;
int peakNum_Merge=0;
for (int iRho=0; iRho<_nRho; iRho++) {
for (int iTheta=0; iTheta<_nTheta; iTheta++) {
if (hough_trans_CS_peak[iTheta][iRho]==1) {
hough_trans_CS_peak[iTheta][iRho]=2;
for (int ar=iTheta-1; ar<=iTheta+1; ar++) {
for (int rx=iRho-1; rx<=iRho+1; rx++) {
int ax;
if (ar<0) { ax=ar+_nTheta; }
else if (ar>=_nTheta) { ax=ar-_nTheta; }
else { ax=ar; }
if ( (ax!=iTheta || rx!=iRho) && rx>=0 && rx<_nRho) {
if (hough_trans_CS_peak[ax][rx]==1) { hough_trans_CS_peak[ax][rx]=0; }
}
}
}
}
if(hough_trans_CS_peak[iTheta][iRho]==2) {
int peak_num = _houghPeakList.size();
//      _houghPeakList.push_back( HoughPeak(_houghSpace->GetBinContent(iTheta+1,iRho+1),iTheta,iRho,exTheta(iTheta,theta_left,theta_right,m_N2),exRho(iRho,rho_down,rho_up,m_N2),true,peak_num+1,_charge) );//store peak
_houghPeakList.push_back( HoughPeak(_houghSpace->GetBinContent(iTheta+1,iRho+1),iTheta,iRho,exTheta(iTheta,theta_left,theta_right,m_N2),exRho(iRho,rho_down,rho_up,m_N2),true,peak_num+1,_charge) );//store peak
peakNum_Merge++;
}
}
}
return peakNum_Merge;
}
*/
//int HoughMap::exRhoBin(double rho){
//  int rhobin = ( (int)(rho-_rhoMin)/((_rhoMax-_rhoMin)/_nRho) );
//  return rhobin;
//}   
//int HoughMap::exThetaBin(double theta){
//  int thetabin = (int)( (theta-_thetaMin)/((_thetaMax-_thetaMin)/_nTheta) );
//  return thetabin;
//}   
/*
   void HoughMap::candiTrack(){
   if( _houghPeakList.size()==0 ) return;
//  cout<<"size "<<_houghPeakList.size()<<endl;
for(int itrack=0;itrack<_houghPeakList.size();itrack++){
if( _houghPeakList[itrack].getisCandiTrack()==true ) combineNeighbor(itrack);    //if track is true do combine 
else    continue;
 
for(int ipeak=itrack+1;ipeak<_houghPeakList.size();ipeak++){
if(m_debug>0) cout<<" compare track peak : "<<itrack<<" "<<ipeak<<endl;
compareTrack_and_Peak(_houghTrackList.back(),_houghPeakList[ipeak]);
}
}
}
*/
 
/*
   void HoughMap::combineNeighbor(int ipeak){
//get the hitlist in the exact peak
// cout<<" ipeak "<<ipeak<<endl;
int theta=_houghPeakList[ipeak].getThetaBin();
int rho  =_houghPeakList[ipeak].getRhoBin();
vector< const HoughHit* > centerHitList = _mapHitList[theta][rho];
for (int px=theta-_peakWidth; px<=theta+_peakWidth; px++) {
for (int py=rho-_peakHigh; py<=rho+_peakHigh; py++) {
int ax;
if (px<0) { ax=px+_nTheta; }
else if (px>=_nTheta) { ax=px-_nTheta; }
else { ax=px; }
if ( (ax!=theta|| py!=rho) && py>=0 && py<_nRho) combine_two_cells(centerHitList,ax,py);
}
}
_houghTrackList.push_back( HoughTrack(_houghPeakList[ipeak],centerHitList , Rho,Theta) );
}
 
void HoughMap::combine_two_cells(vector< const HoughHit* > &centerHitList,int ax,int py){
//cout<<"combine ("<<ax<<","<<py<<endl;
vector< const HoughHit* > aroundHitList_xy= _mapHitList[ax][py];
int size_of_center=centerHitList.size();
int size_of_around=aroundHitList_xy.size();
 
for(int i=0;i<size_of_around;i++){
int same_hit=0;
for(int j=0;j<size_of_center;j++){
if( centerHitList[j]->digi()==aroundHitList_xy[i]->digi() )    {same_hit=1;break;}
}
if(same_hit==0)  {
centerHitList.push_back(aroundHitList_xy[i]);
//  cout<<"push back hit("<<aroundHitList_xy[i]->getLayerId()<<","<<aroundHitList_xy[i]->getWireId()<<")"<<endl;
}
}
}
*/
 
void HoughMap::hitFinding(){
    if(m_debug>0) cout<<"in hit finding "<<endl;
    for(int ipeak=0;ipeak<_houghPeakList.size();ipeak++){
        int hitColNum =  _houghPeakList[ipeak].collectHits(_hitList);
        if( hitColNum < 3 ) _houghPeakList[ipeak].setisCandiTrack(false);
        //_houghPeakList[ipeak].print();
        //_houghPeakList[ipeak].printAllHit();
    }
}
 
void HoughMap::trackFinder(){
    if(m_debug>0) cout<<"in track finder "<<endl;
    //cout<<"houghPeakList_size "<<_houghPeakList.size()<<endl;
    if( _houghPeakList.size()==0 ) return;
    int trkid=0;
    for(int itrack=0;itrack<_houghPeakList.size();itrack++){
        if( _houghPeakList[itrack].getisCandiTrack()==true ){     //if track is true do combine 
            _houghTrackList.push_back( HoughTrack(_houghPeakList[itrack],_houghPeakList[itrack].getHoughHitList(),_houghPeakList[itrack].getRho(),_houghPeakList[itrack].getTheta(), trkid) );
            trkid++;
        }else continue;
 
        for(int ipeak=itrack+1;ipeak<_houghPeakList.size();ipeak++){
            if(_houghPeakList[ipeak].getisCandiTrack())compareTrack_and_Peak(_houghTrackList.back(),_houghPeakList[ipeak]);
        }
    }
    //cout<<"trackList_size............................................."<<_houghTrackList.size()<<endl;
}
 
void HoughMap::compareTrack_and_Peak(HoughTrack &track,HoughPeak& peak){
    int sameHit=0;
    int sizeoftrack=track.getHoughHitList().size();
    int sizeofpeak =peak.getHoughHitList().size();
    //cout<<"track: "<<track.getTrkid()
        //<<", compare peak "<<track.getCenterPeak().getPeakNum()
        //<<" and peak "<<peak.getPeakNum()
        //<<endl;
    //track.getCenterPeak().print();
    //track.getCenterPeak().printAllHit();
    //peak.print();
    //peak.printAllHit();
    for(int i=0;i<sizeoftrack;i++){
        for(int j=0;j<sizeofpeak;j++){
            //if( ((track.getHoughHitList())[i]).getDetectorType() == MDC && (peak.getHoughHitList())[j]->getDetectorType() == MDC && ((track.getHoughHitList())[i]).digi() == ((peak.getHoughHitList())[j])->digi() ) sameHit++;
            //if( ((track.getHoughHitList())[i]).getDetectorType() == CGEM && (peak.getHoughHitList())[j]->getDetectorType() == CGEM && ((track.getHoughHitList())[i]).getCluster() == ((peak.getHoughHitList())[j])->getCluster() ) sameHit++;
            if( ((track.getHoughHitList())[i]).getHitId() == ((peak.getHoughHitList())[j])->getHitId() ){
                if(((track.getHoughHitList())[i]).getSlayerType() !=0 || ((peak.getHoughHitList())[j])->getSlayerType() !=0)continue;
                sameHit++;
                //cout<<" debug digi ("<< ((track.getHoughHitList())[i]).getLayerId()<<" ,"<<((track.getHoughHitList())[i]).getWireId() <<")   ("<< ((peak.getHoughHitList())[j])->getLayerId()<<" ,"<<((peak.getHoughHitList())[j])->getWireId()<<" )"<<endl;
            }
        }
    }
    //int rho_track=track.getRho();
    //int theta_track=track.getTheta();
    //int rho_peak=peak.getRho();
    //int theta_peak=peak.getTheta();
    //if(peak.getTheta()>0.9&&peak.getTheta()<1.8)peak.print();
    if(m_debug>0) cout<<"same hit : "<<(double)sameHit/peak.getHoughHitList().size()<<"  "<<sameHit<<"  "<<peak.getHoughHitList().size()<<endl;
    //if( sameHit>_hitpro*peak.getHoughHitList().size() || (fabs(rho_track-rho_peak)< 0.3 && fabs(theta_track-theta_peak)< 0.3) )  
    if( sameHit>=_hitpro*peak.getHoughHitList().size() ) {
        if(m_debug>0) cout <<"DEBUG peak is out "<<peak.getPeakNum()<<endl;
        peak.setisCandiTrack(false);
        //cout<<"track: "<<track.getTrkid()
            //<<", compare peak "<<track.getCenterPeak().getPeakNum()
            //<<" and peak "<<peak.getPeakNum()
            //<<" share "<<sameHit<<" hits"
            //<<endl; 
        //combineTrack_and_Peak(track,peak);
    }
    else {
        //cout<<sameHit<<"  "<<_hitpro*peak.getHoughHitList().size()<<endl;
        if(m_debug>0)cout<<" DEBUG peak is reserve  "<<peak.getPeakNum()<<endl;
    }
    //cout<<endl;
}
 
/*
   void HoughMap::gravity(){
   int binx,biny,binz;
   _houghSpace->GetMaximumBin(binx,biny,binz);
   double thetabin = _houghSpace->GetXaxis()->GetBinCenter(binx);
   double rhobin = _houghSpace->GetYaxis()->GetBinCenter(biny);
   int ntheta=15;
   int nrho=10;
   double theta_left = thetabin-(ntheta+1/2.)*(M_PI/_nTheta);
   double theta_right= thetabin+(ntheta+1/2.)*(M_PI/_nTheta);
   double rho_down=  rhobin-(nrho+1/2.)*((2*_rhoMax)/_nRho);
   double rho_up =  rhobin+(nrho+1/2.)*((2*_rhoMax)/_nRho);
   int NbinRho=m_N2;
   int NbinTheta=m_N2;
   std::vector<HoughHit>::iterator iter = _hitList.getList().begin();
   TH2D *_houghSpace2 = new TH2D("houghspace2","houghspace2",NbinTheta,theta_left,theta_right,NbinRho,rho_down,rho_up);  //N2
   for(; iter!= _hitList.getList().end(); iter++){
   HoughHit *hit = &(*iter);
   if( _mapHit==1 && hit->getSlayerType()!=0 ) continue;
   double drift_CF = hit->getRTruth();
   hit->makeCir(NbinTheta,theta_left,theta_right,drift_CF);   //N2
   for( int ni=0;ni<NbinTheta;ni++){
   double theta = hit->getCir(ni).getTheta();
   double rho = hit->getCir(ni).getRho();
   double slantOfLine = hit->getCir(ni).getSlant(); 
   int chargeOfHitOnCir = (slantOfLine/fabs(slantOfLine)) * (rho/fabs(rho));
   if( chargeOfHitOnCir == _charge ) continue;  
   if( fabs(slantOfLine)<0.03 ) continue;  
   _houghSpace2->Fill(theta,rho);
   }
   }
   for (int iRho=0; iRho<NbinRho; iRho++) {
   for (int iTheta=0; iTheta<NbinTheta; iTheta++) {
   int   z5=_houghSpace2->GetBinContent(iTheta+1,iRho+1);
   _houghS2[iTheta][iRho]=z5;
   }
   }
   int binx2,biny2,binz2;
   _houghSpace2->GetMaximumBin(binx2,biny2,binz2);
   double thetabin2 = _houghSpace2->GetXaxis()->GetBinCenter(binx2);
   double rhobin2 = _houghSpace2->GetYaxis()->GetBinCenter(biny2);
   Theta=thetabin2;
   Rho=rhobin2;
   Height=_houghSpace2->GetBinContent(binx2,biny2);
   delete _houghSpace2;
   }
   */
/*
   void HoughMap::select_slant(){
   int binx,biny,binz;
   _houghSpace->GetMaximumBin(binx,biny,binz);
   double thetabin = _houghSpace->GetXaxis()->GetBinCenter(binx);
   double rhobin = _houghSpace->GetYaxis()->GetBinCenter(biny);
   vector<int> vec_layer;
   std::vector<HoughHit>::iterator iter1= _hitList.getList().begin();
   for(; iter1!= _hitList.getList().end(); iter1++){
   if( (*iter1).getSlayerType()!=0 || (*iter1).getStyle()!=0 || (*iter1).getCirList()!=0 ) continue;
   vec_layer.push_back( (*iter1).getLayerId() );
   }
 
   if( vec_layer.size()==0 ) return;
   vector<int>::iterator laymax=max_element( vec_layer.begin(),vec_layer.end() );
   int maxLayer=*laymax;
   m_maxlayer=maxLayer;
// cout<<"max layer "<<maxLayer<<endl;
 
vector<int>::iterator iterlay = vec_layer.begin();
for(; iterlay!= vec_layer.end(); iterlay++){
if ( *iterlay==*laymax) {
vec_layer.erase(laymax);
iterlay--;
}
}
vector<int>::iterator laymax2=max_element( vec_layer.begin(),vec_layer.end() );
int maxLayer2=*laymax2;
//  cout<<"max layer2 "<<maxLayer2<<endl;
 
std::vector<HoughHit>::iterator iter = _hitList.getList().begin();
for(; iter!= _hitList.getList().end(); iter++){
HoughHit *hit = &(*iter);
if(  hit->getSlayerType()!=0 || hit->getStyle()!=0 || hit->getCirList()!=0 ) continue;
double slantOfLine= hit->getV()*cos(thetabin) - hit->getU()*sin(thetabin); 
//cout<<"("<<hit->getLayerId()<<","<<hit->getWireId()<<") "<< slantOfLine<<endl;
if( hit->getLayerId()==maxLayer || hit->getLayerId()==maxLayer2){
maxlayer_slant.push_back(slantOfLine);
}
else{
nomaxlayer_slant.push_back(slantOfLine);
nomaxlayerid.push_back(hit->getLayerId());
}
}
}
*/
/*
   void HoughMap::cald_layer(){
//in truth 
double k,b,theta,rho,x_cross,y_cross;
vector<double> vtemp,utemp;
std::vector<HoughHit>::iterator iter = _hitList.getList().begin();
for(int iHit=0;iter!= _hitList.getList().end(); iter++,iHit++){
const HoughHit h = (*iter);
//  if( h.getCirList()!=0 ) continue;
if ( h.digi()->getTrackIndex()>=0 && h.getStyle()==0&& h.getSlayerType()==0 && h.getCirList()==0 && utemp.size()<10)    // ??use 2nd half
{
utemp.push_back(h.getUTruth());
vtemp.push_back(h.getVTruth());
}
}
Leastfit(utemp,vtemp,k,b);
//calcu truth 
//k,b from truth
x_cross = -b/(k+1/k);
y_cross = b/(1+k*k);
rho=sqrt(x_cross*x_cross+y_cross*y_cross);
theta=atan2(y_cross,x_cross);
//  Rho = rho;              //use truth rho, theta
//  Theta =theta;
//
double cirr_track = fabs(1./(Rho));
double cirx_track = (1./Rho)*cos(Theta); 
double ciry_track = (1./Rho)*sin(Theta); 
//cout<<"track center position "<<cirx_track<<" "<<ciry_track<<endl;
std::vector<HoughHit>::iterator iter0= _hitList.getList().begin();
for(; iter0!= _hitList.getList().end(); iter0++){
HoughHit *hit = &(*iter0);
if( hit->getSlayerType()!=0 ) continue;
//  if( hit->getCirList()!=0 ) continue;        // use in learn distribute
//double cirr_hit = hit->getDriftDistTruth();
double cirx_hit = hit->getMidX();
double ciry_hit = hit->getMidY();
double cirr_hit = hit->getDriftDist();
double l1l2 = sqrt( (cirx_hit-cirx_track)*(cirx_hit-cirx_track)+(ciry_hit-ciry_track)*(ciry_hit-ciry_track) );
double deltaD ;
if( l1l2>cirr_track )   deltaD = l1l2-cirr_track-cirr_hit;
if( l1l2<=cirr_track )  deltaD = l1l2-cirr_track+cirr_hit;
hit->setDeltaD(deltaD);
//cal flight length
 
double theta_temp;
double l_temp;
if(cirx_track==0||cirx_hit-cirx_track==0){
theta_temp=0;
}
else{
theta_temp=M_PI-atan2(ciry_hit-ciry_track,cirx_hit-cirx_track)+atan2(ciry_track,cirx_track);
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<<" charge "<<_charge <<" "<<theta_temp<<endl;
if(_charge==-1) {
l_temp=cirr_track*theta_temp;
}
if(_charge==1) {
theta_temp=2*M_PI-theta_temp;
l_temp=cirr_track*(theta_temp);
}
//  cout<<"("<<hit->getLayerId()<<","<<hit->getWireId()<<") "<<l_temp<<endl;
hit->setFltLen(l_temp);
 
//    cout<<"int map deltaD: ("<<hit->getLayerId()<<","<<hit->getWireId()<<")"<< hit->getDeltaD()<<endl;
}
}
*/
/*
   void HoughMap::Leastfit(vector<double> u,vector<double> v,double& k ,double& b){
   int N = u.size();
   double *x=new double[N];
   double *y=new double[N];
   for(int i=0;i<N;i++){
   x[i]=u[i];
   y[i]=v[i];
   }
 
   TF1 *fpol1=new TF1("fpol1","pol1",-50,50);
   TGraph *tg=new TGraph(N,x,y);
   tg->Fit("fpol1","QN");
   double ktemp  =fpol1->GetParameter(1);
   double btemp  =fpol1->GetParameter(0);
   k = ktemp;
   b = btemp;
   delete []x;
   delete []y;
   delete fpol1;
   delete tg;
   }
   */
bool less_hits_in_peak(const HoughPeak& peaka,const HoughPeak& peakb){
    return peaka.peakHeight() > peakb.peakHeight();
}
 
void HoughMap::sortPeaks(){
    std::sort (_houghPeakList.begin(),_houghPeakList.end(),less_hits_in_peak);
}
 
double HoughMap::exRho(int irho,double rhomin,double rhomax,int n){
    //double rho = _rhoMin+(irho+1/2.)*(_rhoMax-_rhoMin)/_nRho;
    double rho = rhomin+(irho+1/2.)*(rhomax-rhomin)/n;
    return rho;                                                                                 
}   
 
double HoughMap::exTheta(int itheta,double thetamin,double thetamax,int n){
    double theta= thetamin+(itheta+1/2.)*(thetamax-thetamin)/n;  
    return theta;
}   
 
void HoughMap::mapDev(vector< vector<int> > vec_hist,double& aver ,double& sigma){
    int count_use=0;
    double sum=0;
    double sum2=0;
    for(int i=0;i<m_N2;i++){
        for(int j=0;j<m_N2;j++){
            int count=vec_hist[i][j];
            if(count!=0){
                count_use++;
                sum+=count;
                sum2+=count*count;
            }
        }
    }
    aver=sum/count_use;
    sigma=sqrt(sum2/count_use-aver*aver);
}
 
void HoughMap::mapDev(TH2D* houghspace,double& aver ,double& sigma){
    int count_use=0;
    double sum=0;
    double sum2=0;
    for(int i=0;i<m_N1;i++){
        for(int j=0;j<m_N1;j++){
            int count=houghspace->GetBinContent(i+1,j+1);
            if(count!=0){
                count_use++;
                sum+=count;
                sum2+=count*count;
            }
        }
    }
    if(count_use != 0){
        aver=sum/count_use;
        sigma=sqrt(sum2/count_use-aver*aver);
    }else{
        aver=0;
        sigma=0;
    }
}
 
double HoughMap::mapDev(TH2D* houghspace,double nRMS){
    int count_use=0;
    double sum=0;
    double sum2=0;
    for(int i=0;i<m_N1;i++){
        for(int j=0;j<m_N1;j++){
            int count=houghspace->GetBinContent(i+1,j+1);
            if(count!=0){
                count_use++;
                sum+=count;
                sum2+=count*count;
            }
        }
    }
    double aver(0),rms(0);
    if(count_use != 0){
        aver=sum/count_use;
        if(count_use==1) rms = sqrt(sum2/count_use-aver*aver);
        else rms = sqrt((sum2-count_use*aver*aver)/(count_use-1));
    }
    double cut = aver + nRMS*rms;
    return cut;
}
 
void HoughMap::printPeak(){
    cout<<"Print Peak in HoughMap sumPeak: "<<_houghPeakList.size()<<endl;
    cout<<"nPeak:"<<getPeakNumber()<<endl;
    vector<HoughPeak>::iterator iter =_houghPeakList.begin();
    for(int i=0;iter!=_houghPeakList.end();iter++,i++){
        //cout<<"count of Peak on HoughMap: "<<(*iter).getHoughHitList().size()<<endl;
        //(*iter).printAllHit();
        (*iter).print();
 
    }
}
 
void HoughMap::printTrack(){
    cout<<"Print Track in HoughMap: "<<endl;
    cout<<"nTrack:"<<getTrackNumber()<<endl;
    vector<HoughTrack>::iterator iter =_houghTrackList.begin();
    for(int i=0;iter!=_houghTrackList.end();iter++,i++){
        //cout<<"Print Track"<<i<<endl;
        //(*iter).printRecHit();
        //(*iter).print();
        (*iter).printHoughTrack();
        (*iter).getCenterPeak().print();
    }
}