HoughGrouping.cc

Go to the documentation of this file.

#include "L1Trigger/DTTriggerPhase2/interface/HoughGrouping.h"

#include "FWCore/Framework/interface/ConsumesCollector.h"
#include "FWCore/ParameterSet/interface/ParameterSet.h"
#include "FWCore/MessageLogger/interface/MessageLogger.h"

#include "DataFormats/Math/interface/CMSUnits.h"

#include <cmath>
#include <memory>

#include "TMath.h"

using namespace std;
using namespace edm;
using namespace cmsdt;
using namespace cms_units::operators;

namespace {
  struct {
    bool operator()(ProtoCand a, ProtoCand b) const {
      unsigned short int sumhqa = 0;
      unsigned short int sumhqb = 0;
      unsigned short int sumlqa = 0;
      unsigned short int sumlqb = 0;
      double sumdista = 0;
      double sumdistb = 0;

      for (unsigned short int lay = 0; lay < 8; lay++) {
        sumhqa += (unsigned short int)a.isThereHitInLayer_[lay];
        sumhqb += (unsigned short int)b.isThereHitInLayer_[lay];
        sumlqa += (unsigned short int)a.isThereNeighBourHitInLayer_[lay];
        sumlqb += (unsigned short int)b.isThereNeighBourHitInLayer_[lay];
        sumdista += a.xDistToPattern_[lay];
        sumdistb += b.xDistToPattern_[lay];
      }

      if (a.nLayersWithHits_ != b.nLayersWithHits_)
        return (a.nLayersWithHits_ > b.nLayersWithHits_);  // number of layers with hits
      else if (sumhqa != sumhqb)
        return (sumhqa > sumhqb);  // number of hq hits
      else if (sumlqa != sumlqb)
        return (sumlqa > sumlqb);  // number of lq hits
      else if (a.nHitsDiff_ != b.nHitsDiff_)
        return (a.nHitsDiff_ < b.nHitsDiff_);  // abs. diff. between SL1 & SL3 hits
      else
        return (sumdista < sumdistb);  // abs. dist. to digis
    }
  } const HoughOrdering;
}  // namespace
// ============================================================================
// Constructors and destructor
// ============================================================================
HoughGrouping::HoughGrouping(const ParameterSet& pset, edm::ConsumesCollector& iC) : MotherGrouping(pset, iC) {
  // Obtention of parameters
  debug_ = pset.getUntrackedParameter<bool>("debug");
  if (debug_)
    LogDebug("HoughGrouping") << "HoughGrouping: constructor";

  // HOUGH TRANSFORM CONFIGURATION
  angletan_ = pset.getUntrackedParameter<double>("angletan");
  anglebinwidth_ = pset.getUntrackedParameter<double>("anglebinwidth");
  posbinwidth_ = pset.getUntrackedParameter<double>("posbinwidth");

  // MAXIMA SEARCH CONFIGURATION
  maxdeltaAngDeg_ = pset.getUntrackedParameter<double>("maxdeltaAngDeg");
  maxdeltaPos_ = pset.getUntrackedParameter<double>("maxdeltaPos");
  upperNumber_ = (unsigned short int)pset.getUntrackedParameter<int>("UpperNumber");
  lowerNumber_ = (unsigned short int)pset.getUntrackedParameter<int>("LowerNumber");

  // HITS ASSOCIATION CONFIGURATION
  maxDistanceToWire_ = pset.getUntrackedParameter<double>("MaxDistanceToWire");

  // CANDIDATE QUALITY CONFIGURATION
  minNLayerHits_ = (unsigned short int)pset.getUntrackedParameter<int>("minNLayerHits");
  minSingleSLHitsMax_ = (unsigned short int)pset.getUntrackedParameter<int>("minSingleSLHitsMax");
  minSingleSLHitsMin_ = (unsigned short int)pset.getUntrackedParameter<int>("minSingleSLHitsMin");
  allowUncorrelatedPatterns_ = pset.getUntrackedParameter<bool>("allowUncorrelatedPatterns");
  minUncorrelatedHits_ = (unsigned short int)pset.getUntrackedParameter<int>("minUncorrelatedHits");

  dtGeomH = iC.esConsumes<DTGeometry, MuonGeometryRecord, edm::Transition::BeginRun>();
}

HoughGrouping::~HoughGrouping() {
  if (debug_)
    LogDebug("HoughGrouping") << "HoughGrouping: destructor" << endl;
}

// ============================================================================
// Main methods (initialise, run, finish)
// ============================================================================
void HoughGrouping::initialise(const edm::EventSetup& iEventSetup) {
  if (debug_)
    LogDebug("HoughGrouping") << "initialise";

  resetAttributes();

  maxrads_ = 0.5 * M_PI - atan(angletan_);
  minangle_ = (double)convertDegToRad(anglebinwidth_);
  halfanglebins_ = round(maxrads_ / minangle_ + 1);
  anglebins_ = (unsigned short int)2 * halfanglebins_;
  oneanglebin_ = maxrads_ / halfanglebins_;

  maxdeltaAng_ = maxdeltaAngDeg_ * 2 * M_PI / 360;

  // Initialisation of anglemap. Posmap depends on the size of the chamber.
  double phi = 0;
  anglemap_ = {};
  for (unsigned short int ab = 0; ab < halfanglebins_; ab++) {
    anglemap_[ab] = phi;
    phi += oneanglebin_;
  }

  phi = (M_PI - maxrads_);
  for (unsigned short int ab = halfanglebins_; ab < anglebins_; ab++) {
    anglemap_[ab] = phi;
    phi += oneanglebin_;
  }

  if (debug_) {
    LogDebug("HoughGrouping")
        << "\nHoughGrouping::ResetAttributes - Information from the initialisation of HoughGrouping:";
    LogDebug("HoughGrouping") << "ResetAttributes - maxrads: " << maxrads_;
    LogDebug("HoughGrouping") << "ResetAttributes - anglebinwidth: " << anglebinwidth_;
    LogDebug("HoughGrouping") << "ResetAttributes - minangle: " << minangle_;
    LogDebug("HoughGrouping") << "ResetAttributes - halfanglebins: " << halfanglebins_;
    LogDebug("HoughGrouping") << "ResetAttributes - anglebins: " << anglebins_;
    LogDebug("HoughGrouping") << "ResetAttributes - oneanglebin: " << oneanglebin_;
    LogDebug("HoughGrouping") << "ResetAttributes - posbinwidth: " << posbinwidth_;
  }

  const MuonGeometryRecord& geom = iEventSetup.get<MuonGeometryRecord>();
  dtGeo_ = &geom.get(dtGeomH);
}

void HoughGrouping::run(edm::Event& iEvent,
                        const edm::EventSetup& iEventSetup,
                        const DTDigiCollection& digis,
                        MuonPathPtrs& outMpath) {
  if (debug_)
    LogDebug("HoughGrouping") << "\nHoughGrouping::run";

  resetAttributes();

  if (debug_)
    LogDebug("HoughGrouping") << "run - Beginning digis' loop...";
  LocalPoint wirePosInLay, wirePosInChamber;
  GlobalPoint wirePosGlob;
  for (DTDigiCollection::DigiRangeIterator dtLayerIdIt = digis.begin(); dtLayerIdIt != digis.end(); dtLayerIdIt++) {
    const DTLayer* lay = dtGeo_->layer((*dtLayerIdIt).first);
    for (DTDigiCollection::const_iterator digiIt = ((*dtLayerIdIt).second).first;
         digiIt != ((*dtLayerIdIt).second).second;
         digiIt++) {
      if (debug_) {
        LogDebug("HoughGrouping") << "\nHoughGrouping::run - Digi number " << idigi_;
        LogDebug("HoughGrouping") << "run - Wheel: " << (*dtLayerIdIt).first.wheel();
        LogDebug("HoughGrouping") << "run - Chamber: " << (*dtLayerIdIt).first.station();
        LogDebug("HoughGrouping") << "run - Sector: " << (*dtLayerIdIt).first.sector();
        LogDebug("HoughGrouping") << "run - Superlayer: " << (*dtLayerIdIt).first.superLayer();
        LogDebug("HoughGrouping") << "run - Layer: " << (*dtLayerIdIt).first.layer();
        LogDebug("HoughGrouping") << "run - Wire: " << (*digiIt).wire();
        LogDebug("HoughGrouping") << "run - First wire: " << lay->specificTopology().firstChannel();
        LogDebug("HoughGrouping") << "run - Last wire: " << lay->specificTopology().lastChannel();
        LogDebug("HoughGrouping") << "run - First wire x: "
                                  << lay->specificTopology().wirePosition(lay->specificTopology().firstChannel());
        LogDebug("HoughGrouping") << "run - Last wire x: "
                                  << lay->specificTopology().wirePosition(lay->specificTopology().lastChannel());
        LogDebug("HoughGrouping") << "run - Cell width: " << lay->specificTopology().cellWidth();
        LogDebug("HoughGrouping") << "run - Cell height: " << lay->specificTopology().cellHeight();
      }
      if ((*dtLayerIdIt).first.superLayer() == 2)
        continue;

      wirePosInLay = LocalPoint(lay->specificTopology().wirePosition((*digiIt).wire()), 0, 0);
      wirePosGlob = lay->toGlobal(wirePosInLay);
      wirePosInChamber = lay->chamber()->toLocal(wirePosGlob);

      if ((*dtLayerIdIt).first.superLayer() == 3) {
        digimap_[(*dtLayerIdIt).first.layer() + 3][(*digiIt).wire()] = DTPrimitive();
        digimap_[(*dtLayerIdIt).first.layer() + 3][(*digiIt).wire()].setTDCTimeStamp((*digiIt).time());
        digimap_[(*dtLayerIdIt).first.layer() + 3][(*digiIt).wire()].setChannelId((*digiIt).wire());
        digimap_[(*dtLayerIdIt).first.layer() + 3][(*digiIt).wire()].setLayerId((*dtLayerIdIt).first.layer());
        digimap_[(*dtLayerIdIt).first.layer() + 3][(*digiIt).wire()].setSuperLayerId((*dtLayerIdIt).first.superLayer());
        digimap_[(*dtLayerIdIt).first.layer() + 3][(*digiIt).wire()].setCameraId((*dtLayerIdIt).first.rawId());
      } else {
        digimap_[(*dtLayerIdIt).first.layer() - 1][(*digiIt).wire()] = DTPrimitive();
        digimap_[(*dtLayerIdIt).first.layer() - 1][(*digiIt).wire()].setTDCTimeStamp((*digiIt).time());
        digimap_[(*dtLayerIdIt).first.layer() - 1][(*digiIt).wire()].setChannelId((*digiIt).wire());
        digimap_[(*dtLayerIdIt).first.layer() - 1][(*digiIt).wire()].setLayerId((*dtLayerIdIt).first.layer());
        digimap_[(*dtLayerIdIt).first.layer() - 1][(*digiIt).wire()].setSuperLayerId((*dtLayerIdIt).first.superLayer());
        digimap_[(*dtLayerIdIt).first.layer() - 1][(*digiIt).wire()].setCameraId((*dtLayerIdIt).first.rawId());
      }

      // Obtaining geometrical info of the chosen chamber
      if (xlowlim_ == 0 && xhighlim_ == 0 && zlowlim_ == 0 && zhighlim_ == 0) {
        thewheel_ = (*dtLayerIdIt).first.wheel();
        thestation_ = (*dtLayerIdIt).first.station();
        thesector_ = (*dtLayerIdIt).first.sector();
        obtainGeometricalBorders(lay);
      }

      if (debug_) {
        LogDebug("HoughGrouping") << "run - X position of the cell (chamber frame of reference): "
                                  << wirePosInChamber.x();
        LogDebug("HoughGrouping") << "run - Y position of the cell (chamber frame of reference): "
                                  << wirePosInChamber.y();
        LogDebug("HoughGrouping") << "run - Z position of the cell (chamber frame of reference): "
                                  << wirePosInChamber.z();
      }

      hitvec_.push_back({wirePosInChamber.x() - 1.05, wirePosInChamber.z()});
      hitvec_.push_back({wirePosInChamber.x() + 1.05, wirePosInChamber.z()});
      nhits_ += 2;

      idigi_++;
    }
  }

  if (debug_) {
    LogDebug("HoughGrouping") << "\nHoughGrouping::run - nhits: " << nhits_;
    LogDebug("HoughGrouping") << "run - idigi: " << idigi_;
  }

  if (hitvec_.empty()) {
    if (debug_)
      LogDebug("HoughGrouping") << "run - No digis present in this chamber.";
    return;
  }

  // Perform the Hough transform of the inputs.
  doHoughTransform();

  // Obtain the maxima
  maxima_ = getMaximaVector();
  resetPosElementsOfLinespace();

  if (maxima_.empty()) {
    if (debug_)
      LogDebug("HoughGrouping") << "run - No good maxima found in this event.";
    return;
  }

  DTChamberId TheChambId(thewheel_, thestation_, thesector_);
  const DTChamber* TheChamb = dtGeo_->chamber(TheChambId);
  std::vector<ProtoCand> cands;

  for (unsigned short int ican = 0; ican < maxima_.size(); ican++) {
    if (debug_)
      LogDebug("HoughGrouping") << "\nHoughGrouping::run - Candidate number: " << ican;
    cands.push_back(associateHits(TheChamb, maxima_.at(ican).first, maxima_.at(ican).second));
  }

  // Now we filter them:
  orderAndFilter(cands, outMpath);
  if (debug_)
    LogDebug("HoughGrouping") << "run - now we have our muonpaths! It has " << outMpath.size() << " elements";

  cands.clear();
  return;
}

void HoughGrouping::finish() {
  if (debug_)
    LogDebug("HoughGrouping") << "finish";
  return;
}

// ============================================================================
// Other methods
// ============================================================================
void HoughGrouping::resetAttributes() {
  if (debug_)
    LogDebug("HoughGrouping") << "ResetAttributes";
  // std::vector's:
  maxima_.clear();
  hitvec_.clear();

  // Integer-type variables:
  spacebins_ = 0;
  idigi_ = 0;
  nhits_ = 0;
  xlowlim_ = 0;
  xhighlim_ = 0;
  zlowlim_ = 0;
  zhighlim_ = 0;
  thestation_ = 0;
  thesector_ = 0;
  thewheel_ = 0;

  // Arrays:
  // NOTE: linespace array is treated and reset separately

  // Maps (dictionaries):
  posmap_.clear();
  for (unsigned short int abslay = 0; abslay < 8; abslay++)
    digimap_[abslay].clear();
}

void HoughGrouping::resetPosElementsOfLinespace() {
  if (debug_)
    LogDebug("HoughGrouping") << "ResetPosElementsOfLinespace";
  for (unsigned short int ab = 0; ab < anglebins_; ab++) {
    linespace_[ab].clear();
  }
  linespace_.clear();
}

void HoughGrouping::obtainGeometricalBorders(const DTLayer* lay) {
  if (debug_)
    LogDebug("HoughGrouping") << "ObtainGeometricalBorders";
  LocalPoint FirstWireLocal(lay->chamber()->superLayer(1)->layer(1)->specificTopology().wirePosition(
                                lay->chamber()->superLayer(1)->layer(1)->specificTopology().firstChannel()),
                            0,
                            0);  // TAKING INFO FROM L1 OF SL1 OF THE CHOSEN CHAMBER
  GlobalPoint FirstWireGlobal = lay->chamber()->superLayer(1)->layer(1)->toGlobal(FirstWireLocal);
  LocalPoint FirstWireLocalCh = lay->chamber()->toLocal(FirstWireGlobal);

  LocalPoint LastWireLocal(lay->chamber()->superLayer(1)->layer(1)->specificTopology().wirePosition(
                               lay->chamber()->superLayer(1)->layer(1)->specificTopology().lastChannel()),
                           0,
                           0);
  GlobalPoint LastWireGlobal = lay->chamber()->superLayer(1)->layer(1)->toGlobal(LastWireLocal);
  LocalPoint LastWireLocalCh = lay->chamber()->toLocal(LastWireGlobal);

  //   unsigned short int upsl = thestation == 4 ? 2 : 3;
  unsigned short int upsl = thestation_ == 4 ? 3 : 3;
  if (debug_)
    LogDebug("HoughGrouping") << "ObtainGeometricalBorders - uppersuperlayer: " << upsl;

  LocalPoint FirstWireLocalUp(lay->chamber()->superLayer(upsl)->layer(4)->specificTopology().wirePosition(
                                  lay->chamber()->superLayer(upsl)->layer(4)->specificTopology().firstChannel()),
                              0,
                              0);  // TAKING INFO FROM L1 OF SL1 OF THE CHOSEN CHAMBER
  GlobalPoint FirstWireGlobalUp = lay->chamber()->superLayer(upsl)->layer(4)->toGlobal(FirstWireLocalUp);
  LocalPoint FirstWireLocalChUp = lay->chamber()->toLocal(FirstWireGlobalUp);

  xlowlim_ = FirstWireLocalCh.x() - lay->chamber()->superLayer(1)->layer(1)->specificTopology().cellWidth() / 2;
  xhighlim_ = LastWireLocalCh.x() + lay->chamber()->superLayer(1)->layer(1)->specificTopology().cellWidth() / 2;
  zlowlim_ = FirstWireLocalChUp.z() - lay->chamber()->superLayer(upsl)->layer(4)->specificTopology().cellHeight() / 2;
  zhighlim_ = LastWireLocalCh.z() + lay->chamber()->superLayer(1)->layer(1)->specificTopology().cellHeight() / 2;

  spacebins_ = round(std::abs(xhighlim_ - xlowlim_) / posbinwidth_);
}

void HoughGrouping::doHoughTransform() {
  if (debug_)
    LogDebug("HoughGrouping") << "DoHoughTransform";
  // First we want to obtain the number of bins in angle that we want. To do so, we will consider at first a maximum angle of
  // (in rad.) pi/2 - arctan(0.3) (i.e. ~73º) and a resolution (width of bin angle) of 2º.

  if (debug_) {
    LogDebug("HoughGrouping") << "DoHoughTransform - maxrads: " << maxrads_;
    LogDebug("HoughGrouping") << "DoHoughTransform - minangle: " << minangle_;
    LogDebug("HoughGrouping") << "DoHoughTransform - halfanglebins: " << halfanglebins_;
    LogDebug("HoughGrouping") << "DoHoughTransform - anglebins: " << anglebins_;
    LogDebug("HoughGrouping") << "DoHoughTransform - oneanglebin: " << oneanglebin_;
    LogDebug("HoughGrouping") << "DoHoughTransform - spacebins: " << spacebins_;
  }

  double rho = 0, phi = 0, sbx = 0;
  double lowinitsb = xlowlim_ + posbinwidth_ / 2;

  // Initialisation
  linespace_.resize(anglebins_, std::vector<unsigned short int>(spacebins_));
  for (unsigned short int ab = 0; ab < anglebins_; ab++) {
    sbx = lowinitsb;
    for (unsigned short int sb = 0; sb < spacebins_; sb++) {
      posmap_[sb] = sbx;
      linespace_[ab][sb] = 0;
      sbx += posbinwidth_;
    }
  }

  // Filling of the double array and actually doing the transform
  for (unsigned short int i = 0; i < hitvec_.size(); i++) {
    for (unsigned short int ab = 0; ab < anglebins_; ab++) {
      phi = anglemap_[ab];
      rho = hitvec_.at(i).first * cos(phi) + hitvec_.at(i).second * sin(phi);
      sbx = lowinitsb - posbinwidth_ / 2;
      for (unsigned short int sb = 0; sb < spacebins_; sb++) {
        if (rho < sbx) {
          linespace_[ab][sb]++;
          break;
        }
        sbx += posbinwidth_;
      }
    }
  }
}

PointsInPlane HoughGrouping::getMaximaVector() {
  if (debug_)
    LogDebug("HoughGrouping") << "getMaximaVector";
  PointTuples tmpvec;

  bool flagsearched = false;
  unsigned short int numbertolookat = upperNumber_;

  while (!flagsearched) {
    for (unsigned short int ab = 0; ab < anglebins_; ab++) {
      for (unsigned short int sb = 0; sb < spacebins_; sb++) {
        if (linespace_[ab][sb] >= numbertolookat)
          tmpvec.push_back({anglemap_[ab], posmap_[sb], linespace_[ab][sb]});
      }
    }
    if (((numbertolookat - 1) < lowerNumber_) || (!tmpvec.empty()))
      flagsearched = true;
    else
      numbertolookat--;
  }

  if (tmpvec.empty()) {
    if (debug_)
      LogDebug("HoughGrouping") << "GetMaximaVector - No maxima could be found";
    PointsInPlane finalvec;
    return finalvec;
  } else {
    PointsInPlane finalvec = findTheMaxima(tmpvec);

    // And now obtain the values of m and n of the lines.
    for (unsigned short int i = 0; i < finalvec.size(); i++)
      finalvec.at(i) = transformPair(finalvec.at(i));
    return finalvec;
  }
}

PointInPlane HoughGrouping::transformPair(const PointInPlane& inputpair) {
  if (debug_)
    LogDebug("HoughGrouping") << "transformPair";
  // input: (ang, pos); output: (m, n)
  if (inputpair.first == 0)
    return {1000, -1000 * inputpair.second};
  else
    return {-1. / tan(inputpair.first), inputpair.second / sin(inputpair.first)};
}

PointsInPlane HoughGrouping::findTheMaxima(PointTuples& inputvec) {
  if (debug_)
    LogDebug("HoughGrouping") << "findTheMaxima";
  bool fullyreduced = false;
  unsigned short int ind = 0;

  std::vector<unsigned short int> chosenvec;
  PointsInPlane resultvec;
  PointInPlane finalpair = {};

  if (debug_)
    LogDebug("HoughGrouping") << "findTheMaxima - prewhile";
  while (!fullyreduced) {
    if (debug_) {
      LogDebug("HoughGrouping") << "\nHoughGrouping::findTheMaxima - New iteration";
      LogDebug("HoughGrouping") << "findTheMaxima - inputvec size: " << inputvec.size();
      LogDebug("HoughGrouping") << "findTheMaxima - ind: " << ind;
      LogDebug("HoughGrouping") << "findTheMaxima - maximum deltaang: " << maxdeltaAng_
                                << " and maximum deltapos: " << maxdeltaPos_;
    }
    chosenvec.clear();
    //calculate distances and check out the ones that are near
    if (debug_)
      LogDebug("HoughGrouping") << "findTheMaxima - Ours have " << get<2>(inputvec.at(ind))
                                << " entries, ang.: " << get<0>(inputvec.at(ind))
                                << " and pos.: " << get<1>(inputvec.at(ind));

    for (unsigned short int j = ind + 1; j < inputvec.size(); j++) {
      if (getTwoDelta(inputvec.at(ind), inputvec.at(j)).first <= maxdeltaAng_ &&
          getTwoDelta(inputvec.at(ind), inputvec.at(j)).second <= maxdeltaPos_) {
        chosenvec.push_back(j);
        if (debug_)
          LogDebug("HoughGrouping") << "findTheMaxima -     - Adding num.  " << j
                                    << " with deltaang: " << getTwoDelta(inputvec.at(ind), inputvec.at(j)).first
                                    << ", and deltapos: " << getTwoDelta(inputvec.at(ind), inputvec.at(j)).second
                                    << " and with " << get<2>(inputvec.at(j))
                                    << " entries, ang.: " << get<0>(inputvec.at(j))
                                    << " and pos.: " << get<1>(inputvec.at(j));
      } else if (debug_)
        LogDebug("HoughGrouping") << "findTheMaxima -     - Ignoring num. " << j
                                  << " with deltaang: " << getTwoDelta(inputvec.at(ind), inputvec.at(j)).first
                                  << ", and deltapos: " << getTwoDelta(inputvec.at(ind), inputvec.at(j)).second
                                  << " and with " << get<2>(inputvec.at(j)) << " entries.";
    }

    if (debug_)
      LogDebug("HoughGrouping") << "findTheMaxima - chosenvecsize: " << chosenvec.size();

    if (chosenvec.empty()) {
      if (ind + 1 >= (unsigned short int)inputvec.size())
        fullyreduced = true;
      if ((get<0>(inputvec.at(ind)) <= maxrads_) || (get<0>(inputvec.at(ind)) >= M_PI - maxrads_))
        resultvec.push_back({get<0>(inputvec.at(ind)), get<1>(inputvec.at(ind))});
      else if (debug_)
        LogDebug("HoughGrouping") << "findTheMaxima -     - Candidate dropped due to an excess in angle";
      ind++;
      continue;
    }

    // Now average them
    finalpair = getAveragePoint(inputvec, ind, chosenvec);

    // Erase the ones you used
    inputvec.erase(inputvec.begin() + ind);
    for (short int j = chosenvec.size() - 1; j > -1; j--) {
      if (debug_)
        LogDebug("HoughGrouping") << "findTheMaxima - erasing index: " << chosenvec.at(j) - 1;
      inputvec.erase(inputvec.begin() + chosenvec.at(j) - 1);
    }

    if (debug_)
      LogDebug("HoughGrouping") << "findTheMaxima - inputvec size: " << inputvec.size();

    // And add the one you calculated:
    if ((finalpair.first <= maxrads_) || (finalpair.first >= M_PI - maxrads_))
      resultvec.push_back(finalpair);
    else if (debug_)
      LogDebug("HoughGrouping") << "findTheMaxima -     - Candidate dropped due to an excess in angle";

    if (ind + 1 >= (unsigned short int)inputvec.size())
      fullyreduced = true;
    if (debug_)
      LogDebug("HoughGrouping") << "findTheMaxima - iteration ends";
    ind++;
  }
  if (debug_)
    LogDebug("HoughGrouping") << "findTheMaxima - postwhile";
  return resultvec;
}

PointInPlane HoughGrouping::getTwoDelta(const PointTuple& pair1, const PointTuple& pair2) {
  if (debug_)
    LogDebug("HoughGrouping") << "getTwoDelta";
  return {abs(get<0>(pair1) - get<0>(pair2)), abs(get<1>(pair1) - get<1>(pair2))};
}

PointInPlane HoughGrouping::getAveragePoint(const PointTuples& inputvec,
                                            unsigned short int firstindex,
                                            const std::vector<unsigned short int>& indexlist) {
  if (debug_)
    LogDebug("HoughGrouping") << "getAveragePoint";
  std::vector<double> xs;
  std::vector<double> ys;
  std::vector<double> ws;
  xs.push_back(get<0>(inputvec.at(firstindex)));
  ys.push_back(get<1>(inputvec.at(firstindex)));
  ws.push_back(exp(get<2>(inputvec.at(firstindex))));
  for (unsigned short int i = 0; i < indexlist.size(); i++) {
    xs.push_back(get<0>(inputvec.at(indexlist.at(i))));
    ys.push_back(get<1>(inputvec.at(indexlist.at(i))));
    ws.push_back(exp(get<2>(inputvec.at(indexlist.at(i)))));
  }
  return {TMath::Mean(xs.begin(), xs.end(), ws.begin()), TMath::Mean(ys.begin(), ys.end(), ws.begin())};
}

ProtoCand HoughGrouping::associateHits(const DTChamber* thechamb, double m, double n) {
  if (debug_)
    LogDebug("HoughGrouping") << "associateHits";
  LocalPoint tmpLocal, AWireLocal, AWireLocalCh, tmpLocalCh, thepoint;
  GlobalPoint tmpGlobal, AWireGlobal;
  double tmpx = 0;
  double distleft = 0;
  double distright = 0;
  unsigned short int tmpwire = 0;
  unsigned short int abslay = 0;
  LATERAL_CASES lat = NONE;
  bool isleft = false;
  bool isright = false;

  ProtoCand returnPC;
  for (auto l = 0; l < NUM_LAYERS_2SL; l++) {
    returnPC.isThereHitInLayer_.push_back(false);
    returnPC.isThereNeighBourHitInLayer_.push_back(false);
    returnPC.xDistToPattern_.push_back(0);
    returnPC.dtHits_.push_back(DTPrimitive());
  }

  if (debug_)
    LogDebug("HoughGrouping") << "associateHits - Beginning SL loop";
  for (unsigned short int sl = 1; sl < 3 + 1; sl++) {
    if (sl == 2)
      continue;
    if (debug_)
      LogDebug("HoughGrouping") << "associateHits - SL: " << sl;

    for (unsigned short int l = 1; l < 4 + 1; l++) {
      if (debug_)
        LogDebug("HoughGrouping") << "associateHits - L: " << l;
      isleft = false;
      isright = false;
      lat = NONE;
      distleft = 0;
      distright = 0;
      if (sl == 1)
        abslay = l - 1;
      else
        abslay = l + 3;
      AWireLocal = LocalPoint(thechamb->superLayer(sl)->layer(l)->specificTopology().wirePosition(
                                  thechamb->superLayer(sl)->layer(l)->specificTopology().firstChannel()),
                              0,
                              0);
      AWireGlobal = thechamb->superLayer(sl)->layer(l)->toGlobal(AWireLocal);
      AWireLocalCh = thechamb->toLocal(AWireGlobal);
      tmpx = (AWireLocalCh.z() - n) / m;

      if ((tmpx <= xlowlim_) || (tmpx >= xhighlim_)) {
        returnPC.dtHits_[abslay] = DTPrimitive();  // empty primitive
        continue;
      }

      thepoint = LocalPoint(tmpx, 0, AWireLocalCh.z());
      tmpwire = thechamb->superLayer(sl)->layer(l)->specificTopology().channel(thepoint);
      if (debug_)
        LogDebug("HoughGrouping") << "associateHits - Wire number: " << tmpwire;
      if (debug_)
        LogDebug("HoughGrouping") << "associateHits - First channel in layer: "
                                  << thechamb->superLayer(sl)->layer(l)->specificTopology().firstChannel();
      if ((digimap_[abslay]).count(tmpwire)) {
        // OK, we have a digi, let's choose the laterality, if we can:
        tmpLocal = LocalPoint(thechamb->superLayer(sl)->layer(l)->specificTopology().wirePosition(tmpwire), 0, 0);
        tmpGlobal = thechamb->superLayer(sl)->layer(l)->toGlobal(tmpLocal);
        tmpLocalCh = thechamb->toLocal(tmpGlobal);

        if (abs(tmpLocalCh.x() - thepoint.x()) >= maxDistanceToWire_) {
          // The distance where lateralities are not put is 0.03 cm, which is a conservative threshold for the resolution of the cells.
          if ((tmpLocalCh.x() - thepoint.x()) > 0)
            lat = LEFT;
          else
            lat = RIGHT;
        }

        // Filling info
        returnPC.nLayersWithHits_++;
        returnPC.isThereHitInLayer_[abslay] = true;
        returnPC.isThereNeighBourHitInLayer_[abslay] = true;
        if (lat == LEFT)
          returnPC.xDistToPattern_[abslay] = abs(tmpx - (tmpLocalCh.x() - 1.05));
        else if (lat == RIGHT)
          returnPC.xDistToPattern_[abslay] = abs(tmpx - (tmpLocalCh.x() + 1.05));
        else
          returnPC.xDistToPattern_[abslay] = abs(tmpx - tmpLocalCh.x());
        returnPC.dtHits_[abslay] = DTPrimitive(digimap_[abslay][tmpwire]);
        returnPC.dtHits_[abslay].setLaterality(lat);
      } else {
        if (debug_)
          LogDebug("HoughGrouping") << "associateHits - No hit in the crossing cell";
        if ((digimap_[abslay]).count(tmpwire - 1))
          isleft = true;
        if ((digimap_[abslay]).count(tmpwire + 1))
          isright = true;
        if (debug_)
          LogDebug("HoughGrouping") << "associateHits - There is in the left: " << (int)isleft;
        if (debug_)
          LogDebug("HoughGrouping") << "associateHits - There is in the right: " << (int)isright;

        if ((isleft) && (!isright)) {
          tmpLocal = LocalPoint(thechamb->superLayer(sl)->layer(l)->specificTopology().wirePosition(tmpwire - 1), 0, 0);
          tmpGlobal = thechamb->superLayer(sl)->layer(l)->toGlobal(tmpLocal);
          tmpLocalCh = thechamb->toLocal(tmpGlobal);

          // Filling info
          returnPC.nLayersWithHits_++;
          returnPC.isThereNeighBourHitInLayer_[abslay] = true;
          returnPC.xDistToPattern_[abslay] = abs(tmpx - (tmpLocalCh.x() + 1.05));
          returnPC.dtHits_[abslay] = DTPrimitive(digimap_[abslay][tmpwire - 1]);
          returnPC.dtHits_[abslay].setLaterality(RIGHT);

        } else if ((!isleft) && (isright)) {
          tmpLocal = LocalPoint(thechamb->superLayer(sl)->layer(l)->specificTopology().wirePosition(tmpwire + 1), 0, 0);
          tmpGlobal = thechamb->superLayer(sl)->layer(l)->toGlobal(tmpLocal);
          tmpLocalCh = thechamb->toLocal(tmpGlobal);

          // Filling info
          returnPC.nLayersWithHits_++;
          returnPC.isThereNeighBourHitInLayer_[abslay] = true;
          returnPC.xDistToPattern_[abslay] = abs(tmpx - (tmpLocalCh.x() - 1.05));
          returnPC.dtHits_[abslay] = DTPrimitive(digimap_[abslay][tmpwire + 1]);
          returnPC.dtHits_[abslay].setLaterality(LEFT);
        } else if ((isleft) && (isright)) {
          LocalPoint tmpLocal_l =
              LocalPoint(thechamb->superLayer(sl)->layer(l)->specificTopology().wirePosition(tmpwire - 1), 0, 0);
          GlobalPoint tmpGlobal_l = thechamb->superLayer(sl)->layer(l)->toGlobal(tmpLocal_l);
          LocalPoint tmpLocalCh_l = thechamb->toLocal(tmpGlobal_l);

          LocalPoint tmpLocal_r =
              LocalPoint(thechamb->superLayer(sl)->layer(l)->specificTopology().wirePosition(tmpwire + 1), 0, 0);
          GlobalPoint tmpGlobal_r = thechamb->superLayer(sl)->layer(l)->toGlobal(tmpLocal_r);
          LocalPoint tmpLocalCh_r = thechamb->toLocal(tmpGlobal_r);

          distleft = std::abs(thepoint.x() - tmpLocalCh_l.x());
          distright = std::abs(thepoint.x() - tmpLocalCh_r.x());

          // Filling info
          returnPC.nLayersWithHits_++;
          returnPC.isThereNeighBourHitInLayer_[abslay] = true;

          returnPC.xDistToPattern_[abslay] = abs(tmpx - (tmpLocalCh.x() - 1.05));
          returnPC.dtHits_[abslay] = DTPrimitive(digimap_[abslay][tmpwire + 1]);
          returnPC.dtHits_[abslay].setLaterality(LEFT);

          if (distleft < distright) {
            returnPC.xDistToPattern_[abslay] = std::abs(tmpx - (tmpLocalCh.x() + 1.05));
            returnPC.dtHits_[abslay] = DTPrimitive(digimap_[abslay][tmpwire - 1]);
            returnPC.dtHits_[abslay].setLaterality(RIGHT);
          } else {
            returnPC.xDistToPattern_[abslay] = std::abs(tmpx - (tmpLocalCh.x() - 1.05));
            returnPC.dtHits_[abslay] = DTPrimitive(digimap_[abslay][tmpwire + 1]);
            returnPC.dtHits_[abslay].setLaterality(LEFT);
          }
        } else {                                     // case where there are no digis
          returnPC.dtHits_[abslay] = DTPrimitive();  // empty primitive
        }
      }
    }
  }

  setDifferenceBetweenSL(returnPC);
  if (debug_) {
    LogDebug("HoughGrouping") << "associateHits - Finishing with the candidate. We have found the following of it:";
    LogDebug("HoughGrouping") << "associateHits - # of layers with hits: " << returnPC.nLayersWithHits_;
    for (unsigned short int lay = 0; lay < 8; lay++) {
      LogDebug("HoughGrouping") << "associateHits - For absolute layer: " << lay;
      LogDebug("HoughGrouping") << "associateHits - # of HQ hits: " << returnPC.isThereHitInLayer_[lay];
      LogDebug("HoughGrouping") << "associateHits - # of LQ hits: " << returnPC.isThereNeighBourHitInLayer_[lay];
    }
    LogDebug("HoughGrouping") << "associateHits - Abs. diff. between SL1 and SL3 hits: " << returnPC.nHitsDiff_;
    for (unsigned short int lay = 0; lay < 8; lay++) {
      LogDebug("HoughGrouping") << "associateHits - For absolute layer: " << lay;
      LogDebug("HoughGrouping") << "associateHits - Abs. distance to digi: " << returnPC.xDistToPattern_[lay];
    }
  }
  return returnPC;
}

void HoughGrouping::setDifferenceBetweenSL(ProtoCand& tupl) {
  if (debug_)
    LogDebug("HoughGrouping") << "setDifferenceBetweenSL";
  short int absres = 0;
  for (unsigned short int lay = 0; lay < 8; lay++) {
    if (tupl.dtHits_[lay].channelId() > 0) {
      if (lay <= 3)
        absres++;
      else
        absres--;
    }
  }

  if (absres >= 0)
    tupl.nHitsDiff_ = absres;
  else
    tupl.nHitsDiff_ = (unsigned short int)(-absres);
}

void HoughGrouping::orderAndFilter(std::vector<ProtoCand>& invector, MuonPathPtrs& outMuonPath) {
  if (debug_)
    LogDebug("HoughGrouping") << "orderAndFilter";
  // 0: # of layers with hits.
  // 1: # of hits of high quality (the expected line crosses the cell).
  // 2: # of hits of low quality (the expected line is in a neighbouring cell).
  // 3: absolute diff. between the number of hits in SL1 and SL3.
  // 4: absolute distance to all hits of the segment.
  // 5: DTPrimitive of the candidate.

  std::vector<unsigned short int> elstoremove;
  // ordering:
  if (debug_)
    LogDebug("HoughGrouping") << "orderAndFilter - First ordering";
  std::sort(invector.begin(), invector.end(), HoughOrdering);

  // Now filtering:
  unsigned short int ind = 0;
  bool filtered = false;
  if (debug_)
    LogDebug("HoughGrouping") << "orderAndFilter - Entering while";
  while (!filtered) {
    if (debug_)
      LogDebug("HoughGrouping") << "\nHoughGrouping::orderAndFilter - New iteration with ind: " << ind;
    elstoremove.clear();
    for (unsigned short int i = ind + 1; i < invector.size(); i++) {
      if (debug_)
        LogDebug("HoughGrouping") << "orderAndFilter - Checking index: " << i;
      for (unsigned short int lay = 0; lay < 8; lay++) {
        if (debug_)
          LogDebug("HoughGrouping") << "orderAndFilter - Checking layer number: " << lay;
        if (invector.at(i).dtHits_[lay].channelId() == invector.at(ind).dtHits_[lay].channelId() &&
            invector.at(ind).dtHits_[lay].channelId() != -1) {
          invector.at(i).nLayersWithHits_--;
          invector.at(i).isThereHitInLayer_[lay] = false;
          invector.at(i).isThereNeighBourHitInLayer_[lay] = false;
          setDifferenceBetweenSL(invector.at(i));
          // We check that if its a different laterality, the best candidate of the two of them changes its laterality to not-known (that is, both).
          if (invector.at(i).dtHits_[lay].laterality() != invector.at(ind).dtHits_[lay].laterality())
            invector.at(ind).dtHits_[lay].setLaterality(NONE);

          invector.at(i).dtHits_[lay] = DTPrimitive();
        }
      }
      if (debug_)
        LogDebug("HoughGrouping")
            << "orderAndFilter - Finished checking all the layers, now seeing if we should remove the "
               "candidate";

      if (!areThereEnoughHits(invector.at(i))) {
        if (debug_)
          LogDebug("HoughGrouping") << "orderAndFilter - This candidate shall be removed!";
        elstoremove.push_back((unsigned short int)i);
      }
    }

    if (debug_)
      LogDebug("HoughGrouping") << "orderAndFilter - We are gonna erase " << elstoremove.size() << " elements";

    for (short int el = (elstoremove.size() - 1); el > -1; el--) {
      invector.erase(invector.begin() + elstoremove.at(el));
    }

    if (ind + 1 == (unsigned short int)invector.size())
      filtered = true;
    else
      std::sort(invector.begin() + ind + 1, invector.end(), HoughOrdering);
    ind++;
  }

  // Ultimate filter: if the remaining do not fill the requirements (configurable through pset arguments), they are removed also.
  for (short int el = (invector.size() - 1); el > -1; el--) {
    if (!areThereEnoughHits(invector.at(el))) {
      invector.erase(invector.begin() + el);
    }
  }

  if (invector.empty()) {
    if (debug_)
      LogDebug("HoughGrouping") << "OrderAndFilter - We do not have candidates with the minimum hits required.";
    return;
  } else if (debug_)
    LogDebug("HoughGrouping") << "OrderAndFilter - At the end, we have only " << invector.size() << " good paths!";

  // Packing dt primitives
  for (unsigned short int i = 0; i < invector.size(); i++) {
    DTPrimitivePtrs ptrPrimitive;
    unsigned short int tmplowfill = 0;
    unsigned short int tmpupfill = 0;
    for (unsigned short int lay = 0; lay < 8; lay++) {
      auto dtAux = std::make_shared<DTPrimitive>(invector.at(i).dtHits_[lay]);
      ptrPrimitive.push_back(std::move(dtAux));
      if (debug_) {
        LogDebug("HoughGrouping") << "\nHoughGrouping::OrderAndFilter - cameraid: " << ptrPrimitive[lay]->cameraId();
        LogDebug("HoughGrouping") << "OrderAndFilter - channelid (GOOD): " << ptrPrimitive[lay]->channelId();
        LogDebug("HoughGrouping") << "OrderAndFilter - channelid (AM):   " << ptrPrimitive[lay]->channelId() - 1;
      }
      // Fixing channel ID to AM conventions...
      if (ptrPrimitive[lay]->channelId() != -1)
        ptrPrimitive[lay]->setChannelId(ptrPrimitive[lay]->channelId() - 1);

      if (ptrPrimitive[lay]->cameraId() > 0) {
        if (lay < 4)
          tmplowfill++;
        else
          tmpupfill++;
      }
    }

    auto ptrMuonPath = std::make_shared<MuonPath>(ptrPrimitive, tmplowfill, tmpupfill);
    outMuonPath.push_back(ptrMuonPath);
    if (debug_) {
      for (unsigned short int lay = 0; lay < 8; lay++) {
        LogDebug("HoughGrouping") << "OrderAndFilter - Final cameraID: "
                                  << outMuonPath.back()->primitive(lay)->cameraId();
        LogDebug("HoughGrouping") << "OrderAndFilter - Final channelID: "
                                  << outMuonPath.back()->primitive(lay)->channelId();
        LogDebug("HoughGrouping") << "OrderAndFilter - Final time: "
                                  << outMuonPath.back()->primitive(lay)->tdcTimeStamp();
      }
    }
  }
  return;
}

bool HoughGrouping::areThereEnoughHits(const ProtoCand& tupl) {
  if (debug_)
    LogDebug("HoughGrouping") << "areThereEnoughHits";
  unsigned short int numhitssl1 = 0;
  unsigned short int numhitssl3 = 0;
  for (unsigned short int lay = 0; lay < 8; lay++) {
    if ((tupl.dtHits_[lay].channelId() > 0) && (lay < 4))
      numhitssl1++;
    else if (tupl.dtHits_[lay].channelId() > 0)
      numhitssl3++;
  }

  if (debug_)
    LogDebug("HoughGrouping") << "areThereEnoughHits - Hits in SL1: " << numhitssl1;
  if (debug_)
    LogDebug("HoughGrouping") << "areThereEnoughHits - Hits in SL3: " << numhitssl3;

  if ((numhitssl1 != 0) && (numhitssl3 != 0)) {  // Correlated candidates
    if ((numhitssl1 + numhitssl3) >= minNLayerHits_) {
      if (numhitssl1 > numhitssl3) {
        return ((numhitssl1 >= minSingleSLHitsMax_) && (numhitssl3 >= minSingleSLHitsMin_));
      } else if (numhitssl3 > numhitssl1) {
        return ((numhitssl3 >= minSingleSLHitsMax_) && (numhitssl1 >= minSingleSLHitsMin_));
      } else
        return true;
    }
  } else if (allowUncorrelatedPatterns_) {  // Uncorrelated candidates
    return ((numhitssl1 + numhitssl3) >= minNLayerHits_);
  }
  return false;
}