File indexing completed on 2025-06-03 00:12:25
0001 #include "L1Trigger/TrackFindingTracklet/interface/TrackMultiplexer.h"
0002
0003 #include <vector>
0004 #include <deque>
0005 #include <set>
0006 #include <numeric>
0007 #include <algorithm>
0008
0009 namespace trklet {
0010
0011 TrackMultiplexer::TrackMultiplexer(const tt::Setup* setup,
0012 const DataFormats* dataFormats,
0013 const ChannelAssignment* channelAssignment,
0014 const Settings* settings,
0015 int region)
0016 : setup_(setup),
0017 dataFormats_(dataFormats),
0018 channelAssignment_(channelAssignment),
0019 settings_(settings),
0020 region_(region),
0021 input_(channelAssignment_->numChannelsTrack()) {
0022
0023 baseUinv2R_ = .5 * settings_->kphi1() / settings_->kr() * pow(2, settings_->rinv_shift());
0024 baseUphiT_ = settings_->kphi1() * pow(2, settings_->phi0_shift());
0025 baseUcot_ = settings_->kz() / settings_->kr() * pow(2, settings_->t_shift());
0026 baseUzT_ = settings_->kz() * pow(2, settings_->z0_shift());
0027 baseUr_ = settings_->kr();
0028 baseUphi_ = settings_->kphi1();
0029 baseUz_ = settings_->kz();
0030
0031 baseLinv2R_ = dataFormats->base(Variable::inv2R, Process::tm);
0032 baseLphiT_ = dataFormats->base(Variable::phiT, Process::tm);
0033 baseLzT_ = dataFormats->base(Variable::zT, Process::tm);
0034 baseLr_ = dataFormats->base(Variable::r, Process::tm);
0035 baseLphi_ = dataFormats->base(Variable::phi, Process::tm);
0036 baseLz_ = dataFormats->base(Variable::z, Process::tm);
0037 baseLcot_ = baseLz_ / baseLr_;
0038
0039 baseHinv2R_ = baseLinv2R_ * pow(2, floor(log2(baseUinv2R_ / baseLinv2R_)));
0040 baseHphiT_ = baseLphiT_ * pow(2, floor(log2(baseUphiT_ / baseLphiT_)));
0041 baseHzT_ = baseLzT_ * pow(2, floor(log2(baseUzT_ / baseLzT_)));
0042 baseHr_ = baseLr_ * pow(2, floor(log2(baseUr_ / baseLr_)));
0043 baseHphi_ = baseLphi_ * pow(2, floor(log2(baseUphi_ / baseLphi_)));
0044 baseHz_ = baseLz_ * pow(2, floor(log2(baseUz_ / baseLz_)));
0045 baseHcot_ = baseLcot_ * pow(2, floor(log2(baseUcot_ / baseLcot_)));
0046
0047 const int baseShiftInvCot = ceil(log2(setup_->outerRadius() / setup_->hybridRangeR())) - setup_->widthDSPbu();
0048 baseInvCot_ = pow(2, baseShiftInvCot);
0049 const int unusedMSBScot =
0050 floor(log2(baseUcot_ * pow(2.0, channelAssignment_->tmWidthCot()) / 2. / setup_->maxCot()));
0051 const int baseShiftScot = channelAssignment_->tmWidthCot() - unusedMSBScot - 1 - setup_->widthAddrBRAM18();
0052 baseScot_ = baseUcot_ * pow(2.0, baseShiftScot);
0053 }
0054
0055
0056 void TrackMultiplexer::consume(const tt::StreamsTrack& streamsTrack, const tt::StreamsStub& streamsStub) {
0057 const int offsetTrack = region_ * channelAssignment_->numChannelsTrack();
0058
0059 int nTracks(0);
0060 int nStubs(0);
0061 for (int channel = 0; channel < channelAssignment_->numChannelsTrack(); channel++) {
0062 const int channelTrack = offsetTrack + channel;
0063 const int offsetStub = channelAssignment_->offsetStub(channelTrack);
0064 const int numProjectionLayers = channelAssignment_->numProjectionLayers(channel);
0065 const tt::StreamTrack& streamTrack = streamsTrack[channelTrack];
0066 input_[channel].reserve(streamTrack.size());
0067 for (int frame = 0; frame < static_cast<int>(streamTrack.size()); frame++) {
0068 if (streamTrack[frame].first.isNull())
0069 continue;
0070 nTracks++;
0071 for (int layer = 0; layer < numProjectionLayers; layer++)
0072 if (streamsStub[offsetStub + layer][frame].first.isNonnull())
0073 nStubs++;
0074 }
0075 }
0076 stubs_.reserve(nStubs + nTracks * channelAssignment_->numSeedingLayers());
0077 tracks_.reserve(nTracks);
0078
0079 for (int channel = 0; channel < channelAssignment_->numChannelsTrack(); channel++) {
0080 const int numP = channelAssignment_->numProjectionLayers(channel);
0081 const int channelTrack = offsetTrack + channel;
0082 const int offsetStub = channelAssignment_->offsetStub(channelTrack);
0083 const tt::StreamTrack& streamTrack = streamsTrack[channelTrack];
0084 std::vector<Track*>& input = input_[channel];
0085 for (int frame = 0; frame < static_cast<int>(streamTrack.size()); frame++) {
0086 const TTTrackRef& ttTrackRef = streamTrack[frame].first;
0087 if (ttTrackRef.isNull()) {
0088 input.push_back(nullptr);
0089 continue;
0090 }
0091
0092 const double offset = region_ * setup_->baseRegion();
0093 double inv2R = digi(-ttTrackRef->rInv() / 2., baseUinv2R_);
0094 const double phi0U = digi(tt::deltaPhi(ttTrackRef->phi() - offset + setup_->hybridRangePhi() / 2.), baseUphiT_);
0095 const double phi0S = digi(phi0U - setup_->hybridRangePhi() / 2., baseUphiT_);
0096 double cot = digi(ttTrackRef->tanL(), baseUcot_);
0097 double z0 = digi(ttTrackRef->z0(), baseUzT_);
0098 double phiT = digi(phi0S + inv2R * digi(setup_->chosenRofPhi(), baseUr_), baseUphiT_);
0099 double zT = digi(z0 + cot * digi(setup_->chosenRofZ(), baseUr_), baseUzT_);
0100
0101 std::vector<Stub*> stubs;
0102 stubs.reserve(channelAssignment_->numSeedingLayers() + numP);
0103 for (int layer = 0; layer < numP; layer++) {
0104 const tt::FrameStub& frameStub = streamsStub[offsetStub + layer][frame];
0105 const TTStubRef& ttStubRef = frameStub.first;
0106 if (ttStubRef.isNull())
0107 continue;
0108
0109 const bool barrel = setup_->barrel(ttStubRef);
0110 const int layerIdTracklet = setup_->trackletLayerId(ttStubRef);
0111 const double basePhi = barrel ? settings_->kphi1() : settings_->kphi(layerIdTracklet);
0112 const double baseRZ = barrel ? settings_->kz(layerIdTracklet) : settings_->kz();
0113 const int widthRZ = barrel ? settings_->zresidbits() : settings_->rresidbits();
0114 TTBV hw(frameStub.second);
0115 const TTBV hwRZ(hw, widthRZ, 0, true);
0116 hw >>= widthRZ;
0117 const TTBV hwPhi(hw, settings_->phiresidbits(), 0, true);
0118 hw >>= settings_->phiresidbits();
0119 const int indexLayerId = setup_->indexLayerId(ttStubRef);
0120 const tt::SensorModule::Type type = setup_->type(ttStubRef);
0121 const int widthR = setup_->tbWidthR(type);
0122 const double baseR = setup_->hybridBaseR(type);
0123 const TTBV hwR(hw, widthR, 0, barrel);
0124 hw >>= widthR;
0125 const TTBV hwStubId(hw, channelAssignment_->tmWidthStubId(), 0, false);
0126 const int stubId = hwStubId.val();
0127 double r = hwR.val(baseR) + (barrel ? setup_->hybridLayerR(indexLayerId) : 0.);
0128 if (type == tt::SensorModule::Disk2S)
0129 r = setup_->disk2SR(indexLayerId, r);
0130 r = digi(r - setup_->chosenRofPhi(), baseUr_);
0131 double phi = hwPhi.val(basePhi);
0132 if (basePhi > baseUphi_)
0133 phi += baseUphi_ / 2.;
0134 double z = digi(hwRZ.val(baseRZ) * (barrel ? 1. : -cot), baseUz_);
0135
0136 bool psTilt = setup_->psModule(ttStubRef);
0137 if (barrel) {
0138 const double posZ = (r + digi(setup_->chosenRofPhi(), baseUr_)) * cot + z0 + z;
0139 const int indexLayerId = setup_->indexLayerId(ttStubRef);
0140 const double limit = setup_->tiltedLayerLimitZ(indexLayerId);
0141 psTilt = std::abs(posZ) < limit;
0142 }
0143 stubs_.emplace_back(ttStubRef, layerIdTracklet, stubId, r, phi, z, psTilt);
0144 stubs.push_back(&stubs_.back());
0145 }
0146 if (setup_->kfUseTTStubParameters()) {
0147 std::vector<TTStubRef> seedTTStubRefs;
0148 seedTTStubRefs.reserve(channelAssignment_->numSeedingLayers());
0149 std::map<int, TTStubRef> mapStubs;
0150 for (TTStubRef& ttStubRef : ttTrackRef->getStubRefs())
0151 mapStubs.emplace(setup_->layerId(ttStubRef), ttStubRef);
0152 for (int layer : channelAssignment_->seedingLayers(ttTrackRef->trackSeedType()))
0153 seedTTStubRefs.push_back(mapStubs[layer]);
0154 const GlobalPoint gp0 = setup_->stubPos(seedTTStubRefs[0]);
0155 const GlobalPoint gp1 = setup_->stubPos(seedTTStubRefs[1]);
0156 const double dH = gp1.perp() - gp0.perp();
0157 const double H1m0 = (gp1.perp() - setup_->chosenRofPhi()) * tt::deltaPhi(gp0.phi() - offset);
0158 const double H0m1 = (gp0.perp() - setup_->chosenRofPhi()) * tt::deltaPhi(gp1.phi() - offset);
0159 const double H3m2 = (gp1.perp() - setup_->chosenRofZ()) * gp0.z();
0160 const double H2m3 = (gp0.perp() - setup_->chosenRofZ()) * gp1.z();
0161 const double dinv2R = inv2R - (gp1.phi() - gp0.phi()) / dH;
0162 const double dcot = cot - (gp1.z() - gp0.z()) / dH;
0163 const double dphiT = phiT - (H1m0 - H0m1) / dH;
0164 const double dzT = zT - (H3m2 - H2m3) / dH;
0165 inv2R -= dinv2R;
0166 cot -= dcot;
0167 phiT -= dphiT;
0168 zT -= dzT;
0169 z0 = zT - cot * setup_->chosenRofZ();
0170
0171 for (Stub* stub : stubs) {
0172 const double dphi = digi(dphiT + stub->r_ * dinv2R, baseUphi_);
0173 const double r = stub->r_ + digi(setup_->chosenRofPhi() - setup_->chosenRofZ(), baseUr_);
0174 const double dz = digi(dzT + r * dcot, baseUz_);
0175 stub->phi_ = digi(stub->phi_ + dphi, baseUphi_);
0176 stub->z_ = digi(stub->z_ + dz, baseUz_);
0177 }
0178 }
0179
0180 for (int seedingLayer = 0; seedingLayer < channelAssignment_->numSeedingLayers(); seedingLayer++) {
0181 const int channelStub = numP + seedingLayer;
0182 const tt::FrameStub& frameStub = streamsStub[offsetStub + channelStub][frame];
0183 const TTStubRef& ttStubRef = frameStub.first;
0184 const int trackletLayerId = setup_->trackletLayerId(ttStubRef);
0185 const int layerId = channelAssignment_->layerId(channel, channelStub);
0186 const int stubId = TTBV(frameStub.second).val(channelAssignment_->tmWidthStubId());
0187 const bool barrel = setup_->barrel(ttStubRef);
0188 double r;
0189 if (barrel) {
0190 const int index = layerId - setup_->offsetLayerId();
0191 const double layer = digi(setup_->hybridLayerR(index), baseUr_);
0192 const double z = digi(z0 + layer * cot, baseUz_);
0193 if (std::abs(z) < digi(setup_->tbBarrelHalfLength(), baseUz_) || index > 0)
0194 r = digi(setup_->hybridLayerR(index) - setup_->chosenRofPhi(), baseUr_);
0195 else {
0196 r = digi(setup_->innerRadius() - setup_->chosenRofPhi(), baseUr_);
0197 }
0198 } else {
0199 const int index = layerId - setup_->offsetLayerId() - setup_->offsetLayerDisks();
0200 const double side = cot < 0. ? -1. : 1.;
0201 const double disk = digi(setup_->hybridDiskZ(index), baseUzT_);
0202 const double invCot = digi(1. / digi(std::abs(cot), baseScot_), baseInvCot_);
0203 const double offset = digi(setup_->chosenRofPhi(), baseUr_);
0204 r = digi((disk - side * z0) * invCot - offset, baseUr_);
0205 }
0206 double phi = 0.;
0207 double z = 0.;
0208
0209 bool psTilt;
0210 if (barrel) {
0211 const int indexLayerId = setup_->indexLayerId(ttStubRef);
0212 const double limit = digi(setup_->tiltedLayerLimitZ(indexLayerId), baseUz_);
0213 const double posR = digi(setup_->hybridLayerR(layerId - setup_->offsetLayerId()), baseUr_);
0214 const double posZ = digi(posR * cot + z0, baseUz_);
0215 psTilt = std::abs(posZ) < limit;
0216 } else
0217 psTilt = true;
0218 stubs_.emplace_back(ttStubRef, trackletLayerId, stubId, r, phi, z, psTilt);
0219 stubs.push_back(&stubs_.back());
0220 }
0221 if (setup_->kfUseTTStubResiduals()) {
0222 for (Stub* stub : stubs) {
0223 const GlobalPoint gp = setup_->stubPos(stub->ttStubRef_);
0224 stub->r_ = gp.perp() - setup_->chosenRofPhi();
0225 stub->phi_ = tt::deltaPhi(gp.phi() - region_ * setup_->baseRegion());
0226 stub->phi_ -= phiT + stub->r_ * inv2R;
0227 stub->z_ = gp.z() - (z0 + gp.perp() * cot);
0228 }
0229 }
0230
0231 if (setup_->kfApplyNonLinearCorrection()) {
0232 for (Stub* stub : stubs) {
0233 const double d = inv2R * (stub->r_ + setup_->chosenRofPhi());
0234 const double dPhi = std::asin(d) - d;
0235 stub->phi_ -= dPhi;
0236 stub->z_ -= dPhi / inv2R * cot;
0237 }
0238 }
0239
0240 bool valid = true;
0241
0242 if (setup_->enableTruncation() && frame >= setup_->numFramesHigh())
0243 valid = false;
0244
0245 if (!dataFormats_->format(Variable::phiT, Process::tm).inRange(phiT, true))
0246 valid = false;
0247 if (!dataFormats_->format(Variable::zT, Process::tm).inRange(zT, true))
0248 valid = false;
0249
0250 for (Stub* stub : stubs) {
0251 if (!dataFormats_->format(Variable::phi, Process::tm).inRange(stub->phi_, true))
0252 stub->valid_ = false;
0253 if (!dataFormats_->format(Variable::z, Process::tm).inRange(stub->z_, true))
0254 stub->valid_ = false;
0255 }
0256
0257 std::set<int> layers, layersPS;
0258 for (Stub* stub : stubs) {
0259 if (!stub->valid_)
0260 continue;
0261 const int layerId = setup_->layerId(stub->ttStubRef_);
0262 layers.insert(layerId);
0263 if (setup_->psModule(stub->ttStubRef_))
0264 layersPS.insert(layerId);
0265 }
0266 if (static_cast<int>(layers.size()) < setup_->kfMinLayers() ||
0267 static_cast<int>(layersPS.size()) < setup_->kfMinLayersPS())
0268 valid = false;
0269
0270 tracks_.emplace_back(ttTrackRef, valid, channel, inv2R, phiT, cot, zT, stubs);
0271 input.push_back(&tracks_.back());
0272 }
0273 }
0274 }
0275
0276
0277 void TrackMultiplexer::produce(tt::StreamsTrack& streamsTrack, tt::StreamsStub& streamsStub) {
0278
0279 for (Track& track : tracks_) {
0280 track.inv2R_ = redigi(track.inv2R_, baseUinv2R_, baseHinv2R_, setup_->widthDSPbu());
0281 track.phiT_ = redigi(track.phiT_, baseUphiT_, baseHphiT_, setup_->widthDSPbu());
0282 track.cot_ = redigi(track.cot_, baseUcot_, baseHcot_, setup_->widthDSPbu());
0283 track.zT_ = redigi(track.zT_, baseUzT_, baseHzT_, setup_->widthDSPbu());
0284 for (Stub* stub : track.stubs_) {
0285 stub->r_ = redigi(stub->r_, baseUr_, baseHr_, setup_->widthDSPbu());
0286 stub->phi_ = redigi(stub->phi_, baseUphi_, baseHphi_, setup_->widthDSPbu());
0287 stub->z_ = redigi(stub->z_, baseUz_, baseHz_, setup_->widthDSPbu());
0288 }
0289 }
0290
0291 for (Track& track : tracks_) {
0292
0293 const double dinv2R = digi(track.inv2R_ - digi(track.inv2R_, baseLinv2R_), baseHinv2R_);
0294 const double dphiT = digi(track.phiT_ - digi(track.phiT_, baseLphiT_), baseHphiT_);
0295 const double dcot = track.cot_ - digi(digi(track.zT_, baseLzT_) / setup_->chosenRofZ(), baseHcot_);
0296 const double dzT = digi(track.zT_ - digi(track.zT_, baseLzT_), baseHzT_);
0297
0298 track.inv2R_ -= dinv2R;
0299 track.phiT_ -= dphiT;
0300 track.cot_ -= dcot;
0301 track.zT_ -= dzT;
0302
0303 if (!dataFormats_->format(Variable::inv2R, Process::tm).inRange(track.inv2R_, true))
0304 track.valid_ = false;
0305 if (!dataFormats_->format(Variable::phiT, Process::tm).inRange(track.phiT_, true))
0306 track.valid_ = false;
0307 if (!dataFormats_->format(Variable::zT, Process::tm).inRange(track.zT_, true))
0308 track.valid_ = false;
0309
0310 for (Stub* stub : track.stubs_) {
0311 const double dphi = digi(dphiT + stub->r_ * dinv2R, baseHphi_);
0312 const double r = stub->r_ + digi(setup_->chosenRofPhi() - setup_->chosenRofZ(), baseHr_);
0313 const double dz = digi(dzT + r * dcot, baseHz_);
0314 stub->phi_ = digi(stub->phi_ + dphi, baseLphi_);
0315 stub->z_ = digi(stub->z_ + dz, baseLz_);
0316
0317 if (!dataFormats_->format(Variable::phi, Process::tm).inRange(stub->phi_, true))
0318 stub->valid_ = false;
0319 if (!dataFormats_->format(Variable::z, Process::tm).inRange(stub->z_, true))
0320 stub->valid_ = false;
0321 }
0322 }
0323
0324 static constexpr int ticksPerGap = 3;
0325 static constexpr int gapPos = 1;
0326 std::vector<std::deque<Track*>> streams(channelAssignment_->numChannelsTrack());
0327 for (int channel = 0; channel < channelAssignment_->numChannelsTrack(); channel++) {
0328 int iTrack(0);
0329 std::deque<Track*>& stream = streams[channel];
0330 const std::vector<Track*>& intput = input_[channel];
0331 for (int tick = 0; iTrack < (int)intput.size(); tick++) {
0332 Track* track = tick % ticksPerGap != gapPos ? intput[iTrack++] : nullptr;
0333 stream.push_back(track && track->valid_ ? track : nullptr);
0334 }
0335 }
0336
0337 for (std::deque<Track*>& stream : streams)
0338 for (auto it = stream.end(); it != stream.begin();)
0339 it = (*--it) ? stream.begin() : stream.erase(it);
0340
0341 std::deque<Track*> accepted;
0342 std::vector<std::deque<Track*>> stacks(channelAssignment_->numChannelsTrack());
0343
0344 while (!std::all_of(streams.begin(), streams.end(), [](const std::deque<Track*>& tracks) {
0345 return tracks.empty();
0346 }) || !std::all_of(stacks.begin(), stacks.end(), [](const std::deque<Track*>& tracks) { return tracks.empty(); })) {
0347
0348 for (int channel = 0; channel < channelAssignment_->numChannelsTrack(); channel++) {
0349 Track* track = pop_front(streams[channel]);
0350 if (track)
0351 stacks[channel].push_back(track);
0352 }
0353
0354 bool nothingToRoute(true);
0355 for (int channel : channelAssignment_->tmMuxOrder()) {
0356 Track* track = pop_front(stacks[channel]);
0357 if (track) {
0358 nothingToRoute = false;
0359 accepted.push_back(track);
0360 break;
0361 }
0362 }
0363 if (nothingToRoute)
0364 accepted.push_back(nullptr);
0365 }
0366
0367 if (setup_->enableTruncation() && static_cast<int>(accepted.size()) > setup_->numFramesHigh())
0368 accepted.resize(setup_->numFramesHigh());
0369
0370 for (auto it = accepted.end(); it != accepted.begin();)
0371 it = (*--it) ? accepted.begin() : accepted.erase(it);
0372
0373 auto frameTrack = [this](Track* track) { return track->valid_ ? track->frame(dataFormats_) : tt::FrameTrack(); };
0374 auto frameStub = [this](Track* track, int layer) {
0375 const auto it = std::find_if(
0376 track->stubs_.begin(), track->stubs_.end(), [layer](Stub* stub) { return stub->layer_ == layer; });
0377 if (!track->valid_ || it == track->stubs_.end() || !(*it)->valid_)
0378 return tt::FrameStub();
0379
0380 return (*it)->frame(dataFormats_);
0381 };
0382 const int offsetStub = region_ * channelAssignment_->tmNumLayers();
0383
0384 streamsTrack[region_].reserve(accepted.size());
0385 for (int layer = 0; layer < channelAssignment_->tmNumLayers(); layer++)
0386 streamsStub[offsetStub + layer].reserve(accepted.size());
0387 for (Track* track : accepted) {
0388 if (!track) {
0389 streamsTrack[region_].emplace_back(tt::FrameTrack());
0390 for (int layer = 0; layer < channelAssignment_->tmNumLayers(); layer++)
0391 streamsStub[offsetStub + layer].emplace_back(tt::FrameStub());
0392 continue;
0393 }
0394 streamsTrack[region_].emplace_back(frameTrack(track));
0395 for (int layer = 0; layer < channelAssignment_->tmNumLayers(); layer++)
0396 streamsStub[offsetStub + layer].emplace_back(frameStub(track, layer));
0397 }
0398 }
0399
0400
0401 template <class T>
0402 T* TrackMultiplexer::pop_front(std::deque<T*>& ts) const {
0403 T* t = nullptr;
0404 if (!ts.empty()) {
0405 t = ts.front();
0406 ts.pop_front();
0407 }
0408 return t;
0409 }
0410
0411
0412 double TrackMultiplexer::redigi(double val, double baseLow, double baseHigh, int widthMultiplier) const {
0413 const double base = std::pow(2, 1 - widthMultiplier);
0414 const double transform = digi(baseLow / baseHigh, base);
0415 return (std::floor(val * transform / baseLow) + .5) * baseHigh;
0416 }
0417
0418 }