Back to home page

Project CMSSW displayed by LXR
 
 

    

File indexing completed on 2025-09-12 10:10:43 

0001 #include "L1Trigger/Phase2L1GMT/interface/KMTFCore.h"
0002 using namespace Phase2L1GMT;
0003 KMTFCore::KMTFCore(const edm::ParameterSet& settings)
0004     : lutService_(new KMTFLUTs(settings.getParameter<std::string>("lutFile"))),
0005       verbose_(settings.getParameter<bool>("verbose")),
0006       initK_(settings.getParameter<std::vector<double> >("initialK")),
0007       initK2_(settings.getParameter<std::vector<double> >("initialK2")),
0008       eLoss_(settings.getParameter<std::vector<double> >("eLoss")),
0009       aPhi_(settings.getParameter<std::vector<double> >("aPhi")),
0010       aPhiB_(settings.getParameter<std::vector<double> >("aPhiB")),
0011       aPhiBNLO_(settings.getParameter<std::vector<double> >("aPhiBNLO")),
0012       bPhi_(settings.getParameter<std::vector<double> >("bPhi")),
0013       bPhiB_(settings.getParameter<std::vector<double> >("bPhiB")),
0014       phiAt2_(settings.getParameter<double>("phiAt2")),
0015 
0016       chiSquareDisp1_(settings.getParameter<std::vector<double> >("chiSquareDisp1")),
0017       chiSquareDisp2_(settings.getParameter<std::vector<double> >("chiSquareDisp2")),
0018       chiSquareDisp3_(settings.getParameter<std::vector<double> >("chiSquareDisp3")),
0019       chiSquareErrADisp1_(settings.getParameter<std::vector<int> >("chiSquareErrADisp1")),
0020       chiSquareErrADisp2_(settings.getParameter<std::vector<int> >("chiSquareErrADisp2")),
0021       chiSquareErrADisp3_(settings.getParameter<std::vector<int> >("chiSquareErrADisp3")),
0022       chiSquareErrBDisp1_(settings.getParameter<std::vector<double> >("chiSquareErrBDisp1")),
0023       chiSquareErrBDisp2_(settings.getParameter<std::vector<double> >("chiSquareErrBDisp2")),
0024       chiSquareErrBDisp3_(settings.getParameter<std::vector<double> >("chiSquareErrBDisp3")),
0025 
0026       chiSquarePrompt1_(settings.getParameter<std::vector<double> >("chiSquarePrompt1")),
0027       chiSquarePrompt2_(settings.getParameter<std::vector<double> >("chiSquarePrompt2")),
0028       chiSquarePrompt3_(settings.getParameter<std::vector<double> >("chiSquarePrompt3")),
0029       chiSquareErrAPrompt1_(settings.getParameter<std::vector<int> >("chiSquareErrAPrompt1")),
0030       chiSquareErrAPrompt2_(settings.getParameter<std::vector<int> >("chiSquareErrAPrompt2")),
0031       chiSquareErrAPrompt3_(settings.getParameter<std::vector<int> >("chiSquareErrAPrompt3")),
0032       chiSquareErrBPrompt1_(settings.getParameter<std::vector<double> >("chiSquareErrBPrompt1")),
0033       chiSquareErrBPrompt2_(settings.getParameter<std::vector<double> >("chiSquareErrBPrompt2")),
0034       chiSquareErrBPrompt3_(settings.getParameter<std::vector<double> >("chiSquareErrBPrompt3")),
0035 
0036       chiSquareCutDispPattern_(settings.getParameter<std::vector<int> >("chiSquareCutDispPattern")),
0037       chiSquareCutOffDisp_(settings.getParameter<std::vector<int> >("chiSquareCutOffDisp")),
0038       chiSquareCutDisp_(settings.getParameter<std::vector<int> >("chiSquareCutDisp")),
0039 
0040       chiSquareCutPromptPattern_(settings.getParameter<std::vector<int> >("chiSquareCutPromptPattern")),
0041       chiSquareCutOffPrompt_(settings.getParameter<std::vector<int> >("chiSquareCutOffPrompt")),
0042       chiSquareCutPrompt_(settings.getParameter<std::vector<int> >("chiSquareCutPrompt")),
0043 
0044       combos4_(settings.getParameter<std::vector<int> >("combos4")),
0045       combos3_(settings.getParameter<std::vector<int> >("combos3")),
0046       combos2_(settings.getParameter<std::vector<int> >("combos2")),
0047       combos1_(settings.getParameter<std::vector<int> >("combos1")),
0048 
0049       useOfflineAlgo_(settings.getParameter<bool>("useOfflineAlgo")),
0050       mScatteringPhi_(settings.getParameter<std::vector<double> >("mScatteringPhi")),
0051       mScatteringPhiB_(settings.getParameter<std::vector<double> >("mScatteringPhiB")),
0052       pointResolutionPhi_(settings.getParameter<double>("pointResolutionPhi")),
0053       pointResolutionPhiB_(settings.getParameter<double>("pointResolutionPhiB")),
0054       pointResolutionPhiBH_(settings.getParameter<std::vector<double> >("pointResolutionPhiBH")),
0055       pointResolutionPhiBL_(settings.getParameter<std::vector<double> >("pointResolutionPhiBL")),
0056       pointResolutionVertex_(settings.getParameter<double>("pointResolutionVertex")),
0057       curvResolution1_(settings.getParameter<std::vector<double> >("curvResolution1")),
0058       curvResolution2_(settings.getParameter<std::vector<double> >("curvResolution2")) {}
0059 
0060 std::pair<l1t::KMTFTrack, l1t::KMTFTrack> KMTFCore::chain(const l1t::MuonStubRef& seed,
0061                                                           const l1t::MuonStubRefVector& stubs) {
0062   std::vector<l1t::KMTFTrack> pretracks;
0063   std::vector<int> combinatorics;
0064   int seedQual;
0065   switch (seed->depthRegion()) {
0066     case 1:
0067       combinatorics = combos1_;
0068       break;
0069     case 2:
0070       combinatorics = combos2_;
0071       break;
0072     case 3:
0073       combinatorics = combos3_;
0074       break;
0075     case 4:
0076       combinatorics = combos4_;
0077       break;
0078     default:
0079       throw cms::Exception("KMTFCore") << "Something really bad happend\n";
0080   }
0081 
0082   l1t::KMTFTrack nullTrack(seed, seed->coord1(), correctedPhiB(seed));
0083   seedQual = seed->quality();
0084   for (const auto& mask : combinatorics) {
0085     l1t::KMTFTrack track(seed, seed->coord1(), correctedPhiB(seed));
0086     int phiB = correctedPhiB(seed);
0087     int charge;
0088     if (phiB == 0)
0089       charge = 0;
0090     else
0091       charge = phiB / fabs(phiB);
0092 
0093     int address = phiB;
0094     if (track.step() == 4 && (fabs(seed->coord2()) > 15))
0095       address = charge * 15 * ap_ufixed<PHIBSCALE, PHIBSCALE_INT>(28.5205658);
0096     if (track.step() == 3 && (fabs(seed->coord2()) > 100))
0097       address = charge * 100 * ap_ufixed<PHIBSCALE, PHIBSCALE_INT>(28.5205658);
0098     if (track.step() == 2 && (fabs(seed->coord2()) > 250))
0099       address = charge * 250 * ap_ufixed<PHIBSCALE, PHIBSCALE_INT>(28.5205658);
0100     int initialK =
0101         int(initK_[seed->depthRegion() - 1] * address / (1 + initK2_[seed->depthRegion() - 1] * charge * address));
0102     if (initialK >= pow(2, BITSCURV - 1))
0103       initialK = pow(2, BITSCURV - 1) - 1;
0104     if (initialK <= -pow(2, BITSCURV - 1))
0105       initialK = -pow(2, BITSCURV - 1) + 1;
0106     track.setCoordinates(seed->depthRegion(), initialK, seed->coord1(), phiB);
0107     if (seed->quality() < 6) {
0108       track.setCoordinates(seed->depthRegion(), initialK, seed->coord1(), 0);
0109     }
0110     if (verbose_) {
0111       edm::LogInfo("KMTFCore") << "Initial state: phiB=" << phiB << " addr=" << address << " K=" << initialK;
0112     }
0113     track.setHitPattern(hitPattern(track));
0114     //set covariance
0115     l1t::KMTFTrack::CovarianceMatrix covariance;
0116     float DK = curvResolution1_[track.step() - 1] + curvResolution2_[track.step() - 1] * initialK * initialK;
0117     if (seed->quality() < 6)
0118       DK = pow(2, 22);
0119     //    DK = pow(2,24);
0120     covariance(0, 0) = DK * 4;
0121     covariance(0, 1) = 0;
0122     covariance(0, 2) = 0;
0123     covariance(1, 0) = 0;
0124     covariance(1, 1) = float(pointResolutionPhi_);
0125     covariance(1, 2) = 0;
0126     covariance(2, 0) = 0;
0127     covariance(2, 1) = 0;
0128     if (!(mask == 1 || mask == 2 || mask == 3 || mask == 4 || mask == 5 || mask == 9 || mask == 6 || mask == 10 ||
0129           mask == 12))
0130       covariance(2, 2) = float(pointResolutionPhiB_);
0131     else {
0132       if (seed->quality() < 6)
0133         covariance(2, 2) = float(pointResolutionPhiBL_[seed->depthRegion() - 1]);
0134       else
0135         covariance(2, 2) = float(pointResolutionPhiBH_[seed->depthRegion() - 1]);
0136     }
0137     track.setCovariance(covariance);
0138 
0139     //
0140     if (verbose_) {
0141       edm::LogInfo("KMTFCore") << "New Kalman fit staring at step=" << track.step() << ", phi=" << track.positionAngle()
0142                                << ", phiB=" << track.bendingAngle() << ", with curvature=" << track.curvature();
0143       edm::LogInfo("KMTFCore") << "BITMASK:" << std::flush;
0144       for (unsigned int i = 0; i < 4; ++i)
0145         edm::LogInfo("KMTFCore") << getBit(mask, i) << std::flush;
0146       edm::LogInfo("KMTFCore") << std::endl;
0147       edm::LogInfo("KMTFCore") << "------------------------------------------------------";
0148       edm::LogInfo("KMTFCore") << "------------------------------------------------------";
0149       edm::LogInfo("KMTFCore") << "------------------------------------------------------";
0150       edm::LogInfo("KMTFCore") << "stubs:";
0151       for (const auto& stub : stubs)
0152         edm::LogInfo("KMTFCore") << "station=" << stub->depthRegion() << " phi=" << stub->coord1()
0153                                  << " phiB=" << correctedPhiB(stub) << " qual=" << stub->quality()
0154                                  << " tag=" << stub->id() << " sector=" << stub->phiRegion()
0155                                  << " wheel=" << stub->etaRegion() << " fineEta= " << stub->eta1() << " "
0156                                  << stub->eta2();
0157       edm::LogInfo("KMTFCore") << "------------------------------------------------------";
0158       edm::LogInfo("KMTFCore") << "------------------------------------------------------";
0159     }
0160 
0161     bool passedU = false;
0162     bool passedV = false;
0163     while (track.step() > 0) {
0164       // muon station 1
0165       if (track.step() == 1) {
0166         track.setCoordinatesAtMuon(track.curvature(), track.positionAngle(), track.bendingAngle());
0167         passedU = estimateChiSquare(track, false);
0168         setRank(track, false);
0169 
0170         if (verbose_)
0171           edm::LogInfo("KMTFCore") << "Calculated Chi2 for displaced track =" << track.approxDispChi2()
0172                                    << "  Passed Cut=" << passedU;
0173         calculateEta(track);
0174         setFourVectors(track);
0175         //calculate coarse eta
0176         //////////////////////
0177         if (verbose_)
0178           edm::LogInfo("KMTFCore") << "Unconstrained PT  in Muon System: pt=" << track.displacedP4().pt();
0179         calculateEta(track);
0180       }
0181 
0182       propagate(track);
0183       if (verbose_)
0184         edm::LogInfo("KMTFCore") << "propagated Coordinates step:" << track.step() << "phi=" << track.positionAngle()
0185                                  << "phiB=" << track.bendingAngle() << "K=" << track.curvature();
0186       if (track.step() > 0)
0187         if (getBit(mask, track.step() - 1)) {
0188           std::pair<bool, uint> bestStub = match(seed, stubs, track.step());
0189           if (verbose_)
0190             edm::LogInfo("KMTFCore") << "Found match =" << bestStub.first << " index=" << bestStub.second
0191                                      << " number of all stubs=" << stubs.size();
0192 
0193           if ((!bestStub.first) || (!update(track, stubs[bestStub.second], mask, seedQual)))
0194             break;
0195           if (verbose_) {
0196             edm::LogInfo("KMTFCore") << "updated Coordinates step:" << track.step() << " phi=" << track.positionAngle()
0197                                      << " phiB=" << track.bendingAngle() << " K=" << track.curvature();
0198           }
0199         }
0200 
0201       if (track.step() == 0) {
0202         track.setCoordinatesAtVertex(track.curvature(), track.positionAngle(), track.bendingAngle());
0203         if (verbose_)
0204           edm::LogInfo("KMTFCore") << " Coordinates before vertex constraint step:" << track.step()
0205                                    << " phi=" << track.phiAtVertex() << " dxy=" << track.dxy()
0206                                    << " K=" << track.curvatureAtVertex();
0207 
0208         //apply vertex constraint for non single tracks
0209         if (track.stubs().size() > 1)
0210           vertexConstraint(track);
0211 
0212         passedV = estimateChiSquare(track, true);
0213         setRank(track, true);
0214 
0215         if (verbose_)
0216           edm::LogInfo("KMTFCore") << "Calculated Chi2 for prompt track =" << track.approxPromptChi2()
0217                                    << "  Passed Cut=" << passedV;
0218 
0219         if (verbose_) {
0220           edm::LogInfo("KMTFCore") << " Coordinates after vertex constraint step:" << track.step()
0221                                    << " phi=" << track.phiAtVertex() << " dxy=" << track.dxy()
0222                                    << " K=" << track.curvatureAtVertex()
0223                                    << "  maximum local chi2=" << track.approxPromptChi2();
0224           edm::LogInfo("KMTFCore") << "------------------------------------------------------";
0225           edm::LogInfo("KMTFCore") << "------------------------------------------------------";
0226         }
0227         setFourVectors(track);
0228         //finally set the displaced or prompt ID
0229         track.setIDFlag(passedV, passedU);
0230 
0231         if (verbose_)
0232           edm::LogInfo("KMTFCore") << "Floating point coordinates at vertex: pt=" << track.pt()
0233                                    << ", eta=" << track.eta() << " phi=" << track.phi();
0234 
0235         pretracks.push_back(track);
0236       }
0237     }
0238   }
0239   if (verbose_) {
0240     if (!pretracks.empty())
0241       edm::LogInfo("KMTFCore") << "-----Kalman Algo at station " << seed->depthRegion() << " (uncleaned)-----";
0242     for (const auto& track : pretracks)
0243       edm::LogInfo("KMTFCore") << "Kalman Track charge=" << track.charge() << " pt=" << track.pt()
0244                                << " hit pattern = " << track.hitPattern() << " eta=" << track.eta()
0245                                << " phi=" << track.phi() << " curvature=" << track.curvatureAtVertex()
0246                                << " curvature STA =" << track.curvatureAtMuon()
0247                                << " stubs=" << int(track.stubs().size()) << " chi2=" << track.approxPromptChi2() << ","
0248                                << track.approxDispChi2() << " rank=" << track.rankPrompt() << "," << track.rankDisp()
0249                                << " pts=" << track.pt() << " " << track.displacedP4().pt() << "   ID=" << track.id();
0250   }
0251   //Now for all the pretracks we need only one vertex constrained and one vertex unconstrained
0252   //so we clean twice
0253   if (verbose_)
0254     edm::LogInfo("KMTFCore") << "Chain Reconstructed " << pretracks.size()
0255                              << " pretracks, now cleaning them separately";
0256 
0257   std::vector<l1t::KMTFTrack> cleanedPrompt = clean(pretracks, seed->depthRegion(), true);
0258   std::vector<l1t::KMTFTrack> cleanedDisp = clean(pretracks, seed->depthRegion(), false);
0259   if (verbose_)
0260     edm::LogInfo("KMTFCore") << "Cleaned Chain tracks prompt=" << cleanedPrompt.size()
0261                              << " displaced=" << cleanedDisp.size();
0262 
0263   if (cleanedPrompt.empty() && cleanedDisp.empty())
0264     return std::make_pair(nullTrack, nullTrack);
0265   else if ((!cleanedPrompt.empty()) && cleanedDisp.empty())
0266     return std::make_pair(cleanedPrompt[0], nullTrack);
0267   else if (cleanedPrompt.empty() && (!cleanedDisp.empty()))
0268     return std::make_pair(nullTrack, cleanedDisp[0]);
0269   else
0270     return std::make_pair(cleanedPrompt[0], cleanedDisp[0]);
0271 }
0272 
0273 std::vector<l1t::KMTFTrack> KMTFCore::clean(const std::vector<l1t::KMTFTrack>& tracks, uint seed, bool vertex) {
0274   std::vector<l1t::KMTFTrack> out;
0275 
0276   std::map<uint, int> infoRank;
0277   std::map<uint, l1t::KMTFTrack> infoTrack;
0278   for (uint i = 1; i <= 15; ++i) {
0279     infoRank[i] = -1;
0280   }
0281 
0282   for (const auto& track : tracks) {
0283     if (vertex) {
0284       if ((track.id() & 0x1) == 0)
0285         continue;
0286       if (verbose_)
0287         edm::LogInfo("KMTFCore") << "Chain Cleaning : Pre Track = pattern = " << track.rankPrompt()
0288                                  << " rank=" << track.hitPattern();
0289       infoRank[track.hitPattern()] = track.rankPrompt();
0290       infoTrack[track.hitPattern()] = track;
0291 
0292     } else {
0293       if ((track.id() & 0x2) == 0)
0294         continue;
0295       infoRank[track.hitPattern()] = track.rankDisp();
0296       infoTrack[track.hitPattern()] = track;
0297     }
0298   }
0299 
0300   int selected = 15;
0301   if (seed == 4)  //station 4 seeded
0302   {
0303     int sel6 = infoRank[10] >= infoRank[12] ? 10 : 12;
0304     int sel5 = infoRank[14] >= infoRank[9] ? 14 : 9;
0305     int sel4 = infoRank[11] >= infoRank[13] ? 11 : 13;
0306     int sel3 = infoRank[sel6] >= infoRank[sel5] ? sel6 : sel5;
0307     int sel2 = infoRank[sel4] >= infoRank[sel3] ? sel4 : sel3;
0308     int sel1 = infoRank[15] >= infoRank[sel2] ? 15 : sel2;
0309     if (vertex)
0310       selected = infoRank[sel1] > 0 ? sel1 : 8;
0311     else
0312       selected = sel1;
0313   }
0314   if (seed == 3)  //station 3 seeded
0315   {
0316     int sel2 = infoRank[5] >= infoRank[6] ? 5 : 6;
0317     int sel1 = infoRank[7] >= infoRank[sel2] ? 7 : sel2;
0318     if (vertex)
0319       selected = infoRank[sel1] > 0 ? sel1 : 4;
0320     else
0321       selected = sel1;
0322   }
0323   if (seed == 2)  //station 2 seeded
0324   {
0325     if (vertex)
0326       selected = infoRank[3] > 0 ? 3 : 2;
0327     else
0328       selected = 3;
0329   }
0330   if (seed == 1)  //station 1 seeded
0331     selected = 1;
0332 
0333   auto search = infoTrack.find(selected);
0334   if (search != infoTrack.end())
0335     out.push_back(search->second);
0336 
0337   return out;
0338 }
0339 
0340 std::pair<bool, uint> KMTFCore::match(const l1t::MuonStubRef& seed, const l1t::MuonStubRefVector& stubs, int step) {
0341   l1t::MuonStubRefVector selected;
0342   std::map<uint, uint> diffInfo;
0343   for (uint i = 0; i < 32; ++i) {
0344     diffInfo[i] = 60000;
0345   }
0346 
0347   int wheel = seed->etaRegion();
0348   int innerWheel = 0;
0349   if (wheel == -2)
0350     innerWheel = -1;
0351   if (wheel == -1)
0352     innerWheel = 0;
0353   if (wheel == 0)
0354     innerWheel = 1982;
0355   if (wheel == 1)
0356     innerWheel = 0;
0357   if (wheel == 2)
0358     innerWheel = 1;
0359   //calculate the distance of all stubs
0360 
0361   for (unsigned int N = 0; N < stubs.size(); ++N) {
0362     const l1t::MuonStubRef& stub = stubs[N];
0363     //Should not be stubs with tag=4 but there are, so skip those
0364     if (verbose_)
0365       edm::LogInfo("KMTFCore") << "testing stub on depth=" << stub->depthRegion() << " for step=" << step;
0366 
0367     if (stub->depthRegion() != step)
0368       continue;
0369     if (verbose_)
0370       edm::LogInfo("KMTFCore") << "Passed";
0371 
0372     uint distance = fabs(wrapAround((seed->coord1() - stub->coord1()) >> 3, 32768));
0373     //if the wheels are not adjacent make this huge
0374     if (!((stub->etaRegion() == wheel) || (stub->etaRegion() == innerWheel)))
0375       distance = 60000;
0376 
0377     diffInfo[N] = distance;
0378   }
0379 
0380   uint s1_1 = matchAbs(diffInfo, 0, 1);
0381   uint s1_2 = matchAbs(diffInfo, 2, 3);
0382   uint s1_3 = matchAbs(diffInfo, 4, 5);
0383   uint s1_4 = matchAbs(diffInfo, 6, 7);
0384   uint s1_5 = matchAbs(diffInfo, 8, 9);
0385   uint s1_6 = matchAbs(diffInfo, 10, 11);
0386   uint s1_7 = matchAbs(diffInfo, 12, 13);
0387   uint s1_8 = matchAbs(diffInfo, 14, 15);
0388   uint s1_9 = matchAbs(diffInfo, 16, 17);
0389   uint s1_10 = matchAbs(diffInfo, 18, 19);
0390   uint s1_11 = matchAbs(diffInfo, 20, 21);
0391   uint s1_12 = matchAbs(diffInfo, 22, 23);
0392   uint s1_13 = matchAbs(diffInfo, 24, 25);
0393   uint s1_14 = matchAbs(diffInfo, 26, 27);
0394   uint s1_15 = matchAbs(diffInfo, 28, 29);
0395   uint s1_16 = matchAbs(diffInfo, 30, 31);
0396 
0397   uint s2_1 = matchAbs(diffInfo, s1_1, s1_2);
0398   uint s2_2 = matchAbs(diffInfo, s1_3, s1_4);
0399   uint s2_3 = matchAbs(diffInfo, s1_5, s1_6);
0400   uint s2_4 = matchAbs(diffInfo, s1_7, s1_8);
0401   uint s2_5 = matchAbs(diffInfo, s1_9, s1_10);
0402   uint s2_6 = matchAbs(diffInfo, s1_11, s1_12);
0403   uint s2_7 = matchAbs(diffInfo, s1_13, s1_14);
0404   uint s2_8 = matchAbs(diffInfo, s1_15, s1_16);
0405 
0406   uint s3_1 = matchAbs(diffInfo, s2_1, s2_2);
0407   uint s3_2 = matchAbs(diffInfo, s2_3, s2_4);
0408   uint s3_3 = matchAbs(diffInfo, s2_5, s2_6);
0409   uint s3_4 = matchAbs(diffInfo, s2_7, s2_8);
0410 
0411   uint s4_1 = matchAbs(diffInfo, s3_1, s3_2);
0412   uint s4_2 = matchAbs(diffInfo, s3_3, s3_4);
0413 
0414   uint s5 = matchAbs(diffInfo, s4_1, s4_2);
0415 
0416   if (diffInfo[s5] != 60000)
0417     return std::make_pair(true, s5);
0418   else
0419     return std::make_pair(false, 0);
0420 }
0421 
0422 int KMTFCore::correctedPhiB(const l1t::MuonStubRef& stub) {
0423   return ap_fixed<BITSPHIB, BITSPHIB, AP_TRN_ZERO, AP_SAT>(ap_ufixed<PHIBSCALE, PHIBSCALE_INT>(28.5205658) *
0424                                                            stub->coord2());
0425 }
0426 
0427 void KMTFCore::propagate(l1t::KMTFTrack& track) {
0428   int K = track.curvature();
0429   int phi = track.positionAngle();
0430   int phiB = track.bendingAngle();
0431   unsigned int step = track.step();
0432 
0433   int charge = 1;
0434   if (K != 0)
0435     charge = K / fabs(K);
0436 
0437   int KBound = K;
0438   if (KBound > pow(2, BITSCURV - 3) - 1)
0439     KBound = pow(2, BITSCURV - 3) - 1;
0440   if (KBound < -(pow(2, BITSCURV - 3) - 1))
0441     KBound = -(pow(2, BITSCURV - 3) - 1);
0442 
0443   int KNew = K;
0444   if (step == 1) {
0445     ap_ufixed<17, 0> eLoss = ap_ufixed<17, 0>(eLoss_[step - 1]);
0446     ap_fixed<BITSCURV - 2, BITSCURV - 2> Kint = ap_fixed<BITSCURV - 2, BITSCURV - 2>(KBound);
0447     ap_fixed<16, 5> eK = eLoss * Kint;
0448     if (charge < 0)
0449       eK = -eK;
0450     ap_fixed<BITSCURV, BITSCURV> KnewInt = ap_fixed<BITSCURV, BITSCURV>(K) - eK * Kint;
0451     KNew = KnewInt;
0452     if (verbose_)
0453       edm::LogInfo("KMTFCore") << "propagate to vertex Kint=" << Kint.to_int() << " ek=" << eK.to_float()
0454                                << " Knew=" << KNew;
0455   }
0456 
0457   //phi propagation
0458   ap_fixed<BITSCURV, BITSCURV> phi11 = ap_fixed<BITSPARAM + 1, 2>(aPhi_[step - 1]) * ap_fixed<BITSCURV, BITSCURV>(K);
0459   ap_fixed<BITSPHIB, BITSPHIB> phi12 =
0460       ap_fixed<BITSPARAM + 1, 2>(-bPhi_[step - 1]) * ap_fixed<BITSPHIB, BITSPHIB>(phiB);
0461 
0462   if (verbose_) {
0463     edm::LogInfo("KMTFCore") << "phi prop = " << K << " * " << ap_fixed<BITSPARAM + 1, 2>(aPhi_[step - 1]).to_float()
0464                              << " = " << phi11.to_int() << ", " << phiB << " * "
0465                              << ap_fixed<BITSPARAM + 1, 2>(-bPhi_[step - 1]).to_float() << " = " << phi12.to_int();
0466   }
0467   int phiNew = ap_fixed<BITSPHI, BITSPHI>(phi + phi11 + phi12);
0468 
0469   //phiB propagation
0470   ap_fixed<BITSCURV, BITSCURV> phiB11 = ap_fixed<BITSPARAM, 1>(aPhiB_[step - 1]) * ap_fixed<BITSCURV, BITSCURV>(K);
0471   ap_fixed<BITSPHIB + 1, BITSPHIB + 1> phiB12 =
0472       ap_ufixed<BITSPARAM + 1, 1>(bPhiB_[step - 1]) * ap_fixed<BITSPHIB, BITSPHIB>(phiB);
0473   int phiBNew = ap_fixed<BITSPHIB, BITSPHIB>(phiB11 + phiB12);
0474   if (verbose_) {
0475     edm::LogInfo("KMTFCore") << "phiB prop = " << K << " * " << ap_fixed<BITSPARAM + 1, 2>(aPhiB_[step - 1]).to_float()
0476                              << " = " << phiB11.to_int() << ", " << phiB << " * "
0477                              << ap_ufixed<BITSPARAM + 1, 1>(bPhiB_[step - 1]).to_float() << " = " << phiB12.to_int();
0478   }
0479 
0480   //Only for the propagation to vertex we use second order;
0481   if (step == 1) {
0482     ap_fixed<10, 4> aPhiB = aPhiB_[step - 1];
0483     ap_ufixed<16, 0> aPhiBNLO = aPhiBNLO_[step - 1];
0484 
0485     ap_fixed<BITSCURV - 2, BITSCURV - 2> Kint = ap_fixed<BITSCURV - 2, BITSCURV - 2>(KBound);
0486     ap_fixed<BITSCURV - 1, BITSCURV - 1> aK = aPhiB * Kint;
0487     ap_fixed<16, 5> eK = aPhiBNLO * Kint;
0488     if (charge < 0)
0489       eK = -eK;
0490     ap_fixed<BITSPHIB + 2, BITSPHIB + 2> DXY = aK + eK * Kint;
0491     //ap_fixed<BITSPHIB+3,BITSPHIB+3> diff = DXY - ap_fixed<BITSPHIB, BITSPHIB>(phiB);
0492 
0493     ap_fixed<BITSPHIB, BITSPHIB, AP_TRN_ZERO, AP_SAT_SYM> diff = DXY - ap_fixed<BITSPHIB, BITSPHIB>(phiB);
0494 
0495     phiBNew = ap_fixed<BITSPHIB, BITSPHIB>(diff);
0496     if (verbose_) {
0497       edm::LogInfo("KMTFCore") << "Vertex phiB prop = " << DXY.to_int() << "(=" << aK.to_int() << " +"
0498                                << (eK * Kint).to_int() << ") - " << ap_fixed<BITSPHIB, BITSPHIB>(phiB).to_int() << " = "
0499                                << phiBNew;
0500     }
0501   }
0502   ///////////////////////////////////////////////////////
0503   //Rest of the stuff  is for the offline version only
0504   //where we want to check what is happening in the covariance matrix
0505 
0506   //Create the transformation matrix
0507   double a[9];
0508   a[0] = 1.;
0509   a[1] = 0.0;
0510   a[2] = 0.0;
0511   a[3] = aPhi_[step - 1];
0512   //  a[3] = 0.0;
0513   a[4] = 1.0;
0514   a[5] = -bPhi_[step - 1];
0515   //a[6]=0.0;
0516   a[6] = aPhiB_[step - 1];
0517   if (step == 1)
0518     a[6] = aPhiB_[step - 1] / 2.0;
0519 
0520   a[7] = 0.0;
0521   a[8] = bPhiB_[step - 1];
0522 
0523   ROOT::Math::SMatrix<double, 3> P(a, 9);
0524 
0525   const std::vector<double>& covLine = track.covariance();
0526   l1t::KMTFTrack::CovarianceMatrix cov(covLine.begin(), covLine.end());
0527   cov = ROOT::Math::Similarity(P, cov);
0528 
0529   //Add the multiple scattering
0530   double phiRMS = mScatteringPhi_[step - 1] * K * K;
0531   double phiBRMS = mScatteringPhiB_[step - 1] * K * K;
0532 
0533   std::vector<double> b(6);
0534   b[0] = 0;
0535   b[1] = 0;
0536   b[2] = phiRMS;
0537   b[3] = 0;
0538   b[4] = 0;
0539   b[5] = phiBRMS;
0540 
0541   reco::Candidate::CovarianceMatrix MS(b.begin(), b.end());
0542 
0543   cov = cov + MS;
0544 
0545   if (verbose_) {
0546     edm::LogInfo("KMTFCore") << "Covariance term for phiB = " << cov(2, 2);
0547     edm::LogInfo("KMTFCore") << "Multiple scattering term for phiB = " << MS(2, 2);
0548   }
0549 
0550   track.setCovariance(cov);
0551   track.setCoordinates(step - 1, KNew, phiNew, phiBNew);
0552 }
0553 
0554 bool KMTFCore::update(l1t::KMTFTrack& track, const l1t::MuonStubRef& stub, int mask, int seedQual) {
0555   //  updateEta(track, stub);
0556   if (useOfflineAlgo_) {
0557     if (mask == 3 || mask == 5 || mask == 9 || mask == 6 || mask == 10 || mask == 12)
0558       return updateOffline(track, stub);
0559     else
0560       return updateOffline1D(track, stub);
0561 
0562   } else
0563     return updateLUT(track, stub, mask, seedQual);
0564 }
0565 
0566 bool KMTFCore::updateOffline(l1t::KMTFTrack& track, const l1t::MuonStubRef& stub) {
0567   int trackK = track.curvature();
0568   int trackPhi = track.positionAngle();
0569   int trackPhiB = track.bendingAngle();
0570 
0571   int phi = stub->coord1();
0572   int phiB = correctedPhiB(stub);
0573 
0574   Vector2 residual;
0575   residual[0] = ap_fixed<BITSPHI, BITSPHI>(phi - trackPhi);
0576   residual[1] = phiB - trackPhiB;
0577 
0578   Matrix23 H;
0579   H(0, 0) = 0.0;
0580   H(0, 1) = 1.0;
0581   H(0, 2) = 0.0;
0582   H(1, 0) = 0.0;
0583   H(1, 1) = 0.0;
0584   H(1, 2) = 1.0;
0585 
0586   const auto r11{stub->quality() < 6 ? pointResolutionPhiBL_[track.step() - 1]
0587                                      : pointResolutionPhiBH_[track.step() - 1]};
0588   const double r[]{pointResolutionPhi_, 0.0, 0.0, r11};
0589   const CovarianceMatrix2 R(r, 4);
0590 
0591   const std::vector<double>& covLine = track.covariance();
0592   const l1t::KMTFTrack::CovarianceMatrix cov(covLine.begin(), covLine.end());
0593   CovarianceMatrix2 S = ROOT::Math::Similarity(H, cov) + R;
0594   if (!S.Invert())
0595     return false;
0596   const Matrix32 Gain = cov * ROOT::Math::Transpose(H) * S;
0597 
0598   track.setKalmanGain(
0599       track.step(), fabs(trackK), Gain(0, 0), Gain(0, 1), Gain(1, 0), Gain(1, 1), Gain(2, 0), Gain(2, 1));
0600 
0601   int KNew = (trackK + int(Gain(0, 0) * residual(0) + Gain(0, 1) * residual(1)));
0602   if (fabs(KNew) > pow(2, BITSCURV - 1))
0603     return false;
0604 
0605   int phiNew = wrapAround(trackPhi + residual(0), pow(2, BITSPHI - 1));
0606   int phiBNew = wrapAround(trackPhiB + int(Gain(2, 0) * residual(0) + Gain(2, 1) * residual(1)), pow(2, BITSPHIB - 1));
0607 
0608   track.setResidual(stub->depthRegion() - 1, fabs(phi - phiNew) + fabs(phiB - phiBNew));
0609 
0610   if (verbose_) {
0611     edm::LogInfo("KMTFCore") << "residual " << phi << " - " << trackPhi << " = " << int(residual[0]) << " " << phiB
0612                              << " - " << trackPhiB << " = " << int(residual[1]);
0613     edm::LogInfo("KMTFCore") << "Gains offline: " << Gain(0, 0) << " " << Gain(0, 1) << " " << Gain(2, 0) << " "
0614                              << Gain(2, 1);
0615     edm::LogInfo("KMTFCore") << " K = " << trackK << " + " << Gain(0, 0) << " * " << residual(0) << " + " << Gain(0, 1)
0616                              << " * " << residual(1);
0617     edm::LogInfo("KMTFCore") << " phiB = " << trackPhiB << " + " << Gain(2, 0) << " * " << residual(0) << " + "
0618                              << Gain(2, 1) << " * " << residual(1);
0619   }
0620 
0621   track.setCoordinates(track.step(), KNew, phiNew, phiBNew);
0622   const auto covNew{cov - Gain * (H * cov)};
0623   l1t::KMTFTrack::CovarianceMatrix c;
0624 
0625   c(0, 0) = covNew(0, 0);
0626   c(0, 1) = covNew(0, 1);
0627   c(0, 2) = covNew(0, 2);
0628   c(1, 0) = covNew(1, 0);
0629   c(1, 1) = covNew(1, 1);
0630   c(1, 2) = covNew(1, 2);
0631   c(2, 0) = covNew(2, 0);
0632   c(2, 1) = covNew(2, 1);
0633   c(2, 2) = covNew(2, 2);
0634   if (verbose_) {
0635     edm::LogInfo("KMTFCore") << "Post Fit Covariance Matrix " << cov(0, 0) << " " << cov(1, 1) << " " << cov(2, 2);
0636   }
0637 
0638   track.setCovariance(c);
0639   track.addStub(stub);
0640   track.setHitPattern(hitPattern(track));
0641 
0642   return true;
0643 }
0644 
0645 bool KMTFCore::updateOffline1D(l1t::KMTFTrack& track, const l1t::MuonStubRef& stub) {
0646   int trackK = track.curvature();
0647   int trackPhi = track.positionAngle();
0648   int trackPhiB = track.bendingAngle();
0649 
0650   int phi = stub->coord1();
0651 
0652   double residual = ap_fixed<BITSPHI, BITSPHI>(phi - trackPhi);
0653 
0654   if (verbose_)
0655     edm::LogInfo("KMTFCore") << "residuals " << phi << " - " << trackPhi << " = " << int(residual);
0656 
0657   Matrix13 H;
0658   H(0, 0) = 0.0;
0659   H(0, 1) = 1.0;
0660   H(0, 2) = 0.0;
0661 
0662   const std::vector<double>& covLine = track.covariance();
0663   l1t::KMTFTrack::CovarianceMatrix cov(covLine.begin(), covLine.end());
0664 
0665   double S = ROOT::Math::Similarity(H, cov)(0, 0) + pointResolutionPhi_;
0666   if (S == 0.0)
0667     return false;
0668   Matrix31 Gain = cov * ROOT::Math::Transpose(H) / S;
0669 
0670   track.setKalmanGain(track.step(), fabs(trackK), Gain(0, 0), 0.0, Gain(1, 0), 0.0, Gain(2, 0), 0.0);
0671 
0672   int KNew = wrapAround(trackK + int(Gain(0, 0) * residual), pow(2, BITSCURV - 1));
0673   int phiNew = wrapAround(trackPhi + residual, pow(2, BITSPHI - 1));
0674   int phiBNew = wrapAround(trackPhiB + int(Gain(2, 0) * residual), pow(2, BITSPHIB - 1));
0675   track.setCoordinates(track.step(), KNew, phiNew, phiBNew);
0676   Matrix33 covNew = cov - Gain * (H * cov);
0677   l1t::KMTFTrack::CovarianceMatrix c;
0678 
0679   if (verbose_) {
0680     edm::LogInfo("KMTFCore") << "phiUpdate: " << int(Gain(0, 0) * residual) << " " << int(Gain(2, 0) * residual);
0681     edm::LogInfo("KMTFCore") << " K = " << trackK << " + " << Gain(0, 0) << " * " << residual;
0682     edm::LogInfo("KMTFCore") << " phiBNew = " << trackPhiB << " + " << Gain(2, 0) << " * " << residual;
0683   }
0684 
0685   c(0, 0) = covNew(0, 0);
0686   c(0, 1) = covNew(0, 1);
0687   c(0, 2) = covNew(0, 2);
0688   c(1, 0) = covNew(1, 0);
0689   c(1, 1) = covNew(1, 1);
0690   c(1, 2) = covNew(1, 2);
0691   c(2, 0) = covNew(2, 0);
0692   c(2, 1) = covNew(2, 1);
0693   c(2, 2) = covNew(2, 2);
0694   track.setCovariance(c);
0695   track.addStub(stub);
0696   track.setHitPattern(hitPattern(track));
0697 
0698   return true;
0699 }
0700 
0701 bool KMTFCore::updateLUT(l1t::KMTFTrack& track, const l1t::MuonStubRef& stub, int mask, int seedQual) {
0702   int trackK = track.curvature();
0703   int trackPhi = track.positionAngle();
0704   int trackPhiB = track.bendingAngle();
0705 
0706   int phi = stub->coord1();
0707   int phiB = correctedPhiB(stub);
0708 
0709   Vector2 residual;
0710   ap_fixed<BITSPHI, BITSPHI> residualPhi = phi - trackPhi;
0711   ap_fixed<BITSPHIB + 1, BITSPHIB + 1> residualPhiB = phiB - trackPhiB;
0712 
0713   if (verbose_)
0714     edm::LogInfo("KMTFCore") << "residual " << phi << " - " << trackPhi << " = " << residualPhi.to_int() << " " << phiB
0715                              << " - " << trackPhiB << " = " << residualPhiB.to_int();
0716 
0717   uint absK = fabs(trackK);
0718   if (absK > pow(2, BITSCURV - 2) - 1)
0719     absK = pow(2, BITSCURV - 2) - 1;
0720 
0721   std::vector<float> GAIN;
0722   if (verbose_) {
0723     edm::LogInfo("KMTFCore") << "Looking up LUTs for mask=" << mask << " with hit pattern=" << track.hitPattern();
0724   }
0725   //For the three stub stuff use only gains 0 and 4
0726   if (!(mask == 3 || mask == 5 || mask == 9 || mask == 6 || mask == 10 || mask == 12)) {
0727     GAIN = lutService_->trackGain(track.step(), track.hitPattern(), absK / 16);
0728     GAIN[1] = 0.0;
0729     GAIN[3] = 0.0;
0730 
0731   } else {
0732     GAIN = lutService_->trackGain2(track.step(), track.hitPattern(), absK / 32, seedQual, stub->quality());
0733   }
0734   if (verbose_) {
0735     edm::LogInfo("KMTFCore") << "Gains (fp): " << GAIN[0] << " " << GAIN[1] << " " << GAIN[2] << " " << GAIN[3];
0736 
0737     if (!(mask == 3 || mask == 5 || mask == 9 || mask == 6 || mask == 10 || mask == 12))
0738       edm::LogInfo("KMTFCore") << "Addr=" << absK / 16
0739                                << "   gain0=" << ap_ufixed<GAIN_0, GAIN_0INT>(GAIN[0]).to_float() << " gain4=-"
0740                                << ap_ufixed<GAIN_4, GAIN_4INT>(GAIN[2]).to_float();
0741     else
0742       edm::LogInfo("KMTFCore") << "Addr=" << absK / 32 << "   " << ap_ufixed<GAIN2_0, GAIN2_0INT>(GAIN[0]).to_float()
0743                                << " -" << ap_ufixed<GAIN2_1, GAIN2_1INT>(GAIN[1]).to_float() << " "
0744                                << ap_ufixed<GAIN2_4, GAIN2_4INT>(GAIN[2]).to_float() << " "
0745                                << ap_ufixed<GAIN2_5, GAIN2_5INT>(GAIN[3]).to_float();
0746   }
0747 
0748   track.setKalmanGain(track.step(), fabs(trackK), GAIN[0], GAIN[1], 1, 0, GAIN[2], GAIN[3]);
0749 
0750   int KNew;
0751   if (!(mask == 3 || mask == 5 || mask == 9 || mask == 6 || mask == 10 || mask == 12)) {
0752     KNew = ap_fixed<BITSPHI + 9, BITSPHI + 9>(ap_fixed<BITSCURV, BITSCURV>(trackK) +
0753                                               ap_ufixed<GAIN_0, GAIN_0INT>(GAIN[0]) * residualPhi);
0754     if (verbose_) {
0755       edm::LogInfo("KMTFCore") << "K = " << KNew << " = " << ap_fixed<BITSCURV, BITSCURV>(trackK).to_int() << " + "
0756                                << ap_ufixed<GAIN_0, GAIN_0INT>(GAIN[0]).to_float() << "*" << residualPhi.to_int();
0757     }
0758   } else {
0759     ap_fixed<BITSPHI + 7, BITSPHI + 7> k11 = ap_ufixed<GAIN2_0, GAIN2_0INT>(GAIN[0]) * residualPhi;
0760     ap_fixed<BITSPHIB + 4, BITSPHIB + 4> k12 = ap_ufixed<GAIN2_1, GAIN2_1INT>(GAIN[1]) * residualPhiB;
0761 
0762     KNew = ap_fixed<BITSPHI + 9, BITSPHI + 9>(ap_fixed<BITSCURV, BITSCURV>(trackK) + k11 - k12);
0763     if (verbose_) {
0764       edm::LogInfo("KMTFCore") << "K = " << KNew << " = " << ap_fixed<BITSCURV, BITSCURV>(trackK).to_int() << " + "
0765                                << k11.to_int() << " + " << k12.to_int();
0766     }
0767   }
0768   if ((KNew > (pow(2, BITSCURV - 1) - 1)) || (KNew < -(pow(2, BITSCURV - 1)))) {
0769     if (verbose_)
0770       edm::LogInfo("KMTFCore") << "K has saturated, track has extremely low energy";
0771     return false;
0772   }
0773   KNew = wrapAround(KNew, pow(2, BITSCURV - 1));
0774   int phiNew = phi;
0775 
0776   //different products for different firmware logic
0777   ap_fixed<BITSPHI + 5, BITSPHI + 5> pbdouble_0 = ap_ufixed<GAIN2_4, GAIN2_4INT>(GAIN[2]) * residualPhi;
0778   ap_fixed<BITSPHIB + 4, BITSPHIB + 4> pb_1 = ap_ufixed<GAIN2_5, GAIN2_5INT>(GAIN[3]) * residualPhiB;
0779   ap_fixed<BITSPHI + 9, BITSPHI + 5> pb_0 = ap_ufixed<GAIN_4, GAIN_4INT>(GAIN[2]) * residualPhi;
0780 
0781   if (verbose_) {
0782     edm::LogInfo("KMTFCore") << "phiupdate " << pb_0.to_float() << " " << pb_1.to_float() << " "
0783                              << pbdouble_0.to_float();
0784   }
0785 
0786   int phiBNew;
0787   if (!(mask == 3 || mask == 5 || mask == 9 || mask == 6 || mask == 10 || mask == 12)) {
0788     phiBNew = ap_fixed<BITSPHI + 8, BITSPHI + 8>(ap_fixed<BITSPHIB, BITSPHIB>(trackPhiB) -
0789                                                  ap_ufixed<GAIN_4, GAIN_4INT>(GAIN[2]) * residualPhi);
0790   } else {
0791     phiBNew = ap_fixed<BITSPHI + 7, BITSPHI + 7>(ap_fixed<BITSPHIB, BITSPHIB>(trackPhiB) + pb_1 - pbdouble_0);
0792   }
0793 
0794   if ((phiBNew > (pow(2, BITSPHIB - 1) - 1)) || (phiBNew < (-pow(2, BITSPHIB - 1))))
0795     return false;
0796 
0797   track.setCoordinates(track.step(), KNew, phiNew, phiBNew);
0798   track.addStub(stub);
0799   track.setHitPattern(hitPattern(track));
0800   if (verbose_) {
0801     edm::LogInfo("KMTFCore") << "Stub station =" << stub->depthRegion();
0802 
0803     edm::LogInfo("KMTFCore") << "Updated Hit Pattern =" << track.hitPattern();
0804   }
0805 
0806   return true;
0807 }
0808 
0809 void KMTFCore::vertexConstraint(l1t::KMTFTrack& track) {
0810   if (useOfflineAlgo_)
0811     vertexConstraintOffline(track);
0812   else
0813     vertexConstraintLUT(track);
0814 }
0815 
0816 void KMTFCore::vertexConstraintOffline(l1t::KMTFTrack& track) {
0817   double residual = -track.dxy();
0818   Matrix13 H;
0819   H(0, 0) = 0;
0820   H(0, 1) = 0;
0821   H(0, 2) = 1;
0822 
0823   const std::vector<double>& covLine = track.covariance();
0824   l1t::KMTFTrack::CovarianceMatrix cov(covLine.begin(), covLine.end());
0825 
0826   double S = (ROOT::Math::Similarity(H, cov))(0, 0) + pointResolutionVertex_;
0827   S = 1.0 / S;
0828   Matrix31 Gain = cov * (ROOT::Math::Transpose(H)) * S;
0829   track.setKalmanGain(track.step(), fabs(track.curvature()), Gain(0, 0), Gain(1, 0), Gain(2, 0));
0830 
0831   if (verbose_) {
0832     edm::LogInfo("KMTFCore") << "sigma3=" << cov(0, 3) << " sigma6=" << cov(3, 3);
0833     edm::LogInfo("KMTFCore") << " K = " << track.curvature() << " + " << Gain(0, 0) << " * " << residual;
0834   }
0835 
0836   int KNew = wrapAround(int(track.curvature() + Gain(0, 0) * residual), pow(2, BITSCURV - 1));
0837   int phiNew = wrapAround(int(track.positionAngle() + Gain(1, 0) * residual), pow(2, BITSPHI));
0838   int dxyNew = wrapAround(int(track.dxy() + Gain(2, 0) * residual), pow(2, BITSPHIB));
0839   if (verbose_)
0840     edm::LogInfo("KMTFCore") << "Post fit impact parameter=" << dxyNew;
0841   track.setCoordinatesAtVertex(KNew, phiNew, -residual);
0842   Matrix33 covNew = cov - Gain * (H * cov);
0843   l1t::KMTFTrack::CovarianceMatrix c;
0844   c(0, 0) = covNew(0, 0);
0845   c(0, 1) = covNew(0, 1);
0846   c(0, 2) = covNew(0, 2);
0847   c(1, 0) = covNew(1, 0);
0848   c(1, 1) = covNew(1, 1);
0849   c(1, 2) = covNew(1, 2);
0850   c(2, 0) = covNew(2, 0);
0851   c(2, 1) = covNew(2, 1);
0852   c(2, 2) = covNew(2, 2);
0853   track.setCovariance(c);
0854   //  track.covariance = track.covariance - Gain*H*track.covariance;
0855 }
0856 
0857 void KMTFCore::vertexConstraintLUT(l1t::KMTFTrack& track) {
0858   double residual = -track.dxy();
0859   uint absK = fabs(track.curvature());
0860   if (absK > pow(2, BITSCURV - 4) - 1)
0861     absK = pow(2, BITSCURV - 4) - 1;
0862 
0863   std::pair<float, float> GAIN = lutService_->vertexGain(track.hitPattern(), absK / 4);
0864   track.setKalmanGain(track.step(), fabs(track.curvature()), GAIN.first, GAIN.second, -1);
0865 
0866   ap_fixed<BITSCURV, BITSCURV> k_0 =
0867       -(ap_ufixed<GAIN_V0, GAIN_V0INT>(fabs(GAIN.first))) * ap_fixed<BITSPHIB, BITSPHIB>(residual);
0868   int KNew = ap_fixed<BITSCURV, BITSCURV>(k_0 + ap_fixed<BITSCURV, BITSCURV>(track.curvature()));
0869 
0870   if (verbose_) {
0871     edm::LogInfo("KMTFCore") << "VERTEX GAIN(" << absK / 4 << ")= -"
0872                              << ap_ufixed<GAIN_V0, GAIN_V0INT>(fabs(GAIN.first)).to_float() << " * "
0873                              << ap_fixed<BITSPHIB, BITSPHIB>(residual).to_int() << " = " << k_0.to_int();
0874   }
0875 
0876   //int p_0 = fp_product(GAIN.second, int(residual), 7);
0877   int p_0 = GAIN.second * int(residual);
0878   int phiNew = wrapAround(track.positionAngle() + p_0, pow(2, BITSPHI - 1));
0879   track.setCoordinatesAtVertex(KNew, phiNew, -residual);
0880 }
0881 
0882 int KMTFCore::hitPattern(const l1t::KMTFTrack& track) {
0883   unsigned int mask = 0;
0884   for (const auto& stub : track.stubs()) {
0885     mask = mask + round(pow(2, stub->depthRegion() - 1));
0886   }
0887   return mask;
0888 }
0889 
0890 int KMTFCore::customBitmask(unsigned int bit1, unsigned int bit2, unsigned int bit3, unsigned int bit4) {
0891   return bit1 * 1 + bit2 * 2 + bit3 * 4 + bit4 * 8;
0892 }
0893 
0894 bool KMTFCore::getBit(int bitmask, int pos) { return (bitmask & (1 << pos)) >> pos; }
0895 
0896 void KMTFCore::setFourVectors(l1t::KMTFTrack& track) {
0897   int etaINT = track.coarseEta();
0898   double lsbEta = M_PI / pow(2, 12);
0899 
0900   int charge = 1;
0901   if (track.curvatureAtVertex() < 0)
0902     charge = -1;
0903 
0904   int ptC = ptLUT(track.curvatureAtVertex());
0905   int ptU = ptLUT(track.curvatureAtMuon());
0906 
0907   //if only one stub return the phi of the stub.
0908   //Also set PT =0 and dxy=0
0909   if (track.stubs().size() == 1) {
0910     ptC = 0;
0911     track.setCoordinatesAtMuon(track.curvatureAtMuon(), track.stubs()[0]->coord1(), track.phiBAtMuon());
0912     track.setCoordinatesAtVertex(track.curvatureAtVertex(), track.phiAtVertex(), 0);
0913   }
0914   track.setPt(ptC, ptU);
0915   //shift the dxy by 10 bits
0916   track.setCoordinatesAtVertex(track.curvatureAtVertex(), track.phiAtVertex(), track.dxy() / 1024);
0917 
0918   //vertex
0919   double pt = ptC * 0.03125;
0920   double phi = (track.phiAtMuon() / 32) * M_PI / pow(2, 12);
0921   double eta = etaINT * lsbEta;
0922   track.setPtEtaPhi(pt, eta, phi);
0923   track.setCharge(charge);
0924   pt = double(ptLUT(track.curvatureAtMuon())) * 0.03125;
0925   track.setPtEtaPhiDisplaced(pt, eta, phi);
0926 }
0927 
0928 bool KMTFCore::estimateChiSquare(l1t::KMTFTrack& track, bool vertex) {
0929   int K;
0930   uint chi = 0;
0931   uint chiErr = 0;
0932 
0933   //exception for 1 stub tracks and vertex constraint
0934   //displaced track not allowed / prompt are allowed
0935   if (track.stubs().size() == 1) {
0936     if (!vertex)
0937       return false;
0938     else {
0939       track.setApproxChi2(127, chiErr, vertex);
0940       return true;
0941     }
0942   }
0943 
0944   std::vector<double> prop;
0945   std::vector<double> propErrB;
0946   std::vector<int> propErrA;
0947   std::vector<int> cut;
0948 
0949   const l1t::MuonStubRef& innerStub = track.stubs()[track.stubs().size() - 1];
0950 
0951   if (vertex) {
0952     K = track.curvatureAtVertex();
0953     if (innerStub->depthRegion() == 1) {
0954       prop = chiSquarePrompt1_;
0955       propErrA = chiSquareErrAPrompt1_;
0956       propErrB = chiSquareErrBPrompt1_;
0957     } else if (innerStub->depthRegion() == 2) {
0958       prop = chiSquarePrompt2_;
0959       propErrA = chiSquareErrAPrompt2_;
0960       propErrB = chiSquareErrBPrompt2_;
0961 
0962     } else if (innerStub->depthRegion() == 3) {
0963       prop = chiSquarePrompt3_;
0964       propErrA = chiSquareErrAPrompt3_;
0965       propErrB = chiSquareErrBPrompt3_;
0966     }
0967   } else {
0968     K = track.curvatureAtMuon();
0969     if (innerStub->depthRegion() == 1) {
0970       prop = chiSquareDisp1_;
0971       propErrA = chiSquareErrADisp1_;
0972       propErrB = chiSquareErrBDisp1_;
0973 
0974     } else if (innerStub->depthRegion() == 2) {
0975       prop = chiSquareDisp2_;
0976       propErrA = chiSquareErrADisp2_;
0977       propErrB = chiSquareErrBDisp2_;
0978 
0979     } else if (innerStub->depthRegion() == 3) {
0980       prop = chiSquareDisp3_;
0981       propErrA = chiSquareErrADisp3_;
0982       propErrB = chiSquareErrBDisp3_;
0983     }
0984   }
0985 
0986   ap_fixed<BITSCURV, BITSCURV> Kdig = K;
0987   ap_fixed<BITSCURV - 3, BITSCURV - 3> Kshifted = Kdig >> 4;
0988   ap_ufixed<BITSCURV - 4, BITSCURV - 4> absK = 0;
0989 
0990   if (Kshifted < 0)
0991     absK = (-Kshifted);
0992   else
0993     absK = (Kshifted);
0994 
0995   for (unsigned int i = 0; i < (track.stubs().size() - 1); i++) {
0996     const l1t::MuonStubRef& stub = track.stubs()[i];
0997 
0998     int diffPhi = ap_fixed<BITSPHI, BITSPHI>(stub->coord1() - innerStub->coord1()) >> 4;
0999     int diffPhiB = ap_fixed<BITSPHI, BITSPHI>(correctedPhiB(stub) - correctedPhiB(innerStub)) >> 4;
1000     if (vertex)
1001       diffPhiB = 0;
1002 
1003     if (verbose_)
1004       edm::LogInfo("KMTFCore") << "Error propagation coefficients A="
1005                                << propErrA[stub->depthRegion() - innerStub->depthRegion() - 1]
1006                                << " B=" << propErrB[stub->depthRegion() - innerStub->depthRegion() - 1] << " BK = "
1007                                << uint(ap_fixed<8, 2>(propErrB[stub->depthRegion() - innerStub->depthRegion() - 1]) *
1008                                        (absK >> 4));
1009     uint positionError = propErrA[stub->depthRegion() - innerStub->depthRegion() - 1];
1010     if (stub->quality() < 6 || innerStub->quality() < 6)
1011       positionError = positionError * 2;
1012 
1013     uint err =
1014         positionError + uint(ap_fixed<8, 2>(propErrB[stub->depthRegion() - innerStub->depthRegion() - 1]) * absK);
1015     ap_fixed<8, 2> propC = ap_fixed<8, 2>(prop[stub->depthRegion() - innerStub->depthRegion() - 1]);
1016     ap_fixed<BITSCURV - 3, BITSCURV - 3> AK = -propC * Kshifted;
1017     int delta = diffPhi + diffPhiB + AK;
1018     uint absDelta = delta < 0 ? -delta : delta;
1019     chi = chi + absDelta;
1020     chiErr = chiErr + err;
1021     if (verbose_) {
1022       edm::LogInfo("KMTFCore") << "Chi Square stub for track with pattern=" << track.hitPattern()
1023                                << "   inner stub depth=" << innerStub->depthRegion() << "-> AK=" << int(AK)
1024                                << " stubDepth=" << stub->depthRegion() << " diff1=" << diffPhi << " diff2=" << diffPhiB
1025                                << " delta=" << absDelta << " absK=" << uint(absK) << " err=" << err;
1026     }
1027   }
1028   if (verbose_) {
1029     edm::LogInfo("KMTFCore") << "Chi Square =" << chi << " ChiSquare Error = " << chiErr;
1030   }
1031 
1032   track.setApproxChi2(chi, chiErr, vertex);
1033   if (chi > chiErr)
1034     return false;
1035   return true;
1036 }
1037 
1038 void KMTFCore::setRank(l1t::KMTFTrack& track, bool vertex) {
1039   uint chi = 0;
1040   if (vertex)
1041     chi = track.approxPromptChi2() > 127 ? 127 : track.approxPromptChi2();
1042   else
1043     chi = track.approxDispChi2() > 127 ? 127 : track.approxDispChi2();
1044 
1045   uint Q = 0;
1046   for (const auto& stub : track.stubs()) {
1047     if (stub->quality() > 2)
1048       Q = Q + 2;
1049     else
1050       Q = Q + 1;
1051   }
1052   uint rank = Q * 4 - chi + 125;
1053 
1054   //exception for  track 1100
1055   if (hitPattern(track) == customBitmask(0, 0, 1, 1)) {
1056     rank = 1;
1057   }
1058 
1059   //Exception for one stub tracks.
1060   if (track.stubs().size() == 1)
1061     rank = 0;
1062 
1063   if (verbose_)
1064     edm::LogInfo("KMTFCore") << "Rank Calculated for vertex=" << vertex << "  = " << rank;
1065   track.setRank(rank, vertex);
1066 }
1067 
1068 int KMTFCore::wrapAround(int value, int maximum) {
1069   if (value > maximum - 1)
1070     return wrapAround(value - 2 * maximum, maximum);
1071   if (value < -maximum)
1072     return wrapAround(value + 2 * maximum, maximum);
1073   return value;
1074 }
1075 
1076 int KMTFCore::encode(bool ownwheel, int sector, int tag) {
1077   int wheel = ownwheel ? 1 : 0;
1078   int phi = 0;
1079   if (sector == 1)
1080     phi = 1;
1081   if (sector == -1)
1082     phi = 2;
1083   int addr = (wheel << 4) + (phi << 2) + tag;
1084   return addr;
1085 }
1086 
1087 std::pair<bool, uint> KMTFCore::getByCode(const std::vector<l1t::KMTFTrack>& tracks, int mask) {
1088   for (uint i = 0; i < tracks.size(); ++i) {
1089     edm::LogInfo("KMTFCore") << "Code=" << tracks[i].hitPattern() << ", track=" << mask;
1090     if (tracks[i].hitPattern() == mask)
1091       return std::make_pair(true, i);
1092   }
1093   return std::make_pair(false, 0);
1094 }
1095 
1096 uint KMTFCore::twosCompToBits(int q) {
1097   if (q >= 0)
1098     return q;
1099   else
1100     return (~q) + 1;
1101 }
1102 
1103 uint KMTFCore::etaStubRank(const l1t::MuonStubRef& stub) {
1104   if (stub->etaQuality() == 3)
1105     return 0;
1106   if (stub->etaQuality() == 0)
1107     return 0;
1108   return (stub->etaQuality());
1109 }
1110 
1111 void KMTFCore::calculateEta(l1t::KMTFTrack& track) {
1112   int wheel = track.stubs()[0]->etaRegion();
1113   int sign = 1;
1114   if (wheel < 0)
1115     sign = -1;
1116   uint nstubs = track.stubs().size();
1117   if (nstubs <= 1) {
1118     track.setCoarseEta(0);
1119     return;
1120   }
1121   uint mask = 0;
1122 
1123   for (unsigned int i = 0; i < track.stubs().size(); ++i) {
1124     mask = mask | ((uint(fabs(track.stubs()[i]->etaRegion()) + 1) << (2 * (track.stubs()[i]->depthRegion() - 1))));
1125   }
1126   if (verbose_)
1127     edm::LogInfo("KMTFCore") << "Mask  = " << mask;
1128 
1129   track.setCoarseEta(sign * lutService_->coarseEta(mask));
1130   if (verbose_)
1131     edm::LogInfo("KMTFCore") << "Coarse Eta mask=" << mask << " set = " << sign * lutService_->coarseEta(mask);
1132   track.setFineEta(0);
1133 }
1134 
1135 uint KMTFCore::matchAbs(std::map<uint, uint>& info, uint i, uint j) {
1136   if (info[i] < info[j])
1137     return i;
1138   else
1139     return j;
1140 }
1141 
1142 int KMTFCore::ptLUT(int K) {
1143   float lsb = 1.25 / float(1 << (BITSCURV - 1));
1144   float FK = fabs(K);
1145   int pt = 0;
1146   if (FK > 8191)
1147     FK = 8191;
1148   if (FK < 103)
1149     FK = 103;
1150   FK = FK * lsb;
1151   if (FK == 0) {
1152     pt = 8191;
1153   } else {
1154     float ptF = 1.0 / FK;  //ct
1155     pt = int(ptF / 0.03125);
1156   }
1157 
1158   return pt;
1159 }
[ Source navigation ] [ Diff markup ] [ Identifier search ] [ general search ]

This page was automatically generated by the 2.2.1 LXR engine.

The LXR team
Valid CSS 2.1! Valid HTML 4.01!