Back to home page

Project CMSSW displayed by LXR
 
 

    

File indexing completed on 2024-07-12 23:00:02 

0001 #include <cmath>
0002 #include "L1Trigger/L1TMuonBarrel/interface/L1TMuonBarrelKalmanAlgo.h"
0003 #include "ap_int.h"
0004 #include "ap_fixed.h"
0005 
0006 L1TMuonBarrelKalmanAlgo::L1TMuonBarrelKalmanAlgo(const edm::ParameterSet& settings)
0007     : verbose_(settings.getParameter<bool>("verbose")),
0008       lutService_(new L1TMuonBarrelKalmanLUTs(settings.getParameter<std::string>("lutFile"))),
0009       initK_(settings.getParameter<std::vector<double> >("initialK")),
0010       initK2_(settings.getParameter<std::vector<double> >("initialK2")),
0011       eLoss_(settings.getParameter<std::vector<double> >("eLoss")),
0012       aPhi_(settings.getParameter<std::vector<double> >("aPhi")),
0013       aPhiB_(settings.getParameter<std::vector<double> >("aPhiB")),
0014       aPhiBNLO_(settings.getParameter<std::vector<double> >("aPhiBNLO")),
0015       bPhi_(settings.getParameter<std::vector<double> >("bPhi")),
0016       bPhiB_(settings.getParameter<std::vector<double> >("bPhiB")),
0017       phiAt2_(settings.getParameter<double>("phiAt2")),
0018 
0019       chiSquare_(settings.getParameter<std::vector<double> >("chiSquare")),
0020       chiSquareCutPattern_(settings.getParameter<std::vector<int> >("chiSquareCutPattern")),
0021       chiSquareCutCurv_(settings.getParameter<std::vector<int> >("chiSquareCutCurvMax")),
0022       chiSquareCut_(settings.getParameter<std::vector<int> >("chiSquareCut")),
0023       trackComp_(settings.getParameter<std::vector<double> >("trackComp")),
0024       trackCompErr1_(settings.getParameter<std::vector<double> >("trackCompErr1")),
0025       trackCompErr2_(settings.getParameter<std::vector<double> >("trackCompErr2")),
0026       trackCompPattern_(settings.getParameter<std::vector<int> >("trackCompCutPattern")),
0027       trackCompCutCurv_(settings.getParameter<std::vector<int> >("trackCompCutCurvMax")),
0028       trackCompCut_(settings.getParameter<std::vector<int> >("trackCompCut")),
0029       chiSquareCutTight_(settings.getParameter<std::vector<int> >("chiSquareCutTight")),
0030       combos4_(settings.getParameter<std::vector<int> >("combos4")),
0031       combos3_(settings.getParameter<std::vector<int> >("combos3")),
0032       combos2_(settings.getParameter<std::vector<int> >("combos2")),
0033       combos1_(settings.getParameter<std::vector<int> >("combos1")),
0034 
0035       useOfflineAlgo_(settings.getParameter<bool>("useOfflineAlgo")),
0036       mScatteringPhi_(settings.getParameter<std::vector<double> >("mScatteringPhi")),
0037       mScatteringPhiB_(settings.getParameter<std::vector<double> >("mScatteringPhiB")),
0038       pointResolutionPhi_(settings.getParameter<double>("pointResolutionPhi")),
0039       pointResolutionPhiB_(settings.getParameter<double>("pointResolutionPhiB")),
0040       pointResolutionPhiBH_(settings.getParameter<std::vector<double> >("pointResolutionPhiBH")),
0041       pointResolutionPhiBL_(settings.getParameter<std::vector<double> >("pointResolutionPhiBL")),
0042       pointResolutionVertex_(settings.getParameter<double>("pointResolutionVertex")),
0043       useNewQualityCalculation_(settings.getParameter<bool>("useNewQualityCalculation"))
0044 
0045 {}
0046 
0047 std::pair<bool, uint> L1TMuonBarrelKalmanAlgo::getByCode(const L1MuKBMTrackCollection& tracks, int mask) {
0048   for (uint i = 0; i < tracks.size(); ++i) {
0049     printf("Code=%d, track=%d\n", tracks[i].hitPattern(), mask);
0050     if (tracks[i].hitPattern() == mask)
0051       return std::make_pair(true, i);
0052   }
0053   return std::make_pair(false, 0);
0054 }
0055 
0056 l1t::RegionalMuonCand L1TMuonBarrelKalmanAlgo::convertToBMTF(const L1MuKBMTrack& track) {
0057   //  int  K = fabs(track.curvatureAtVertex());
0058 
0059   //calibration
0060   int sign, signValid;
0061 
0062   if (track.curvatureAtVertex() == 0) {
0063     sign = 0;
0064     signValid = 0;
0065   } else if (track.curvatureAtVertex() > 0) {
0066     sign = 0;
0067     signValid = 1;
0068   } else {
0069     sign = 1;
0070     signValid = 1;
0071   }
0072 
0073   // if (K<22)
0074   //   K=22;
0075 
0076   // if (K>4095)
0077   //   K=4095;
0078 
0079   int pt = ptLUT(track.curvatureAtVertex());
0080 
0081   // int  K2 = fabs(track.curvatureAtMuon());
0082   // if (K2<22)
0083   //   K2=22;
0084 
0085   // if (K2>4095)
0086   //   K2=4095;
0087   int pt2 = ptLUT(track.curvatureAtMuon()) / 2;
0088   int eta = track.hasFineEta() ? track.fineEta() : track.coarseEta();
0089 
0090   //  int phi2 = track.phiAtMuon()>>2;
0091   //  float phi_f = float(phi2);
0092   //double kPhi = 57.2958/0.625/1024.;
0093   //int phi = 24+int(floor(kPhi*phi_f));
0094   //  if (phi >  69) phi =  69;
0095   //  if (phi < -8) phi = -8;
0096   int phi2 = track.phiAtMuon() >> 2;
0097   int tmp = fp_product(0.0895386, phi2, 14);
0098   int phi = 24 + tmp;
0099 
0100   int processor = track.sector();
0101   int HF = track.hasFineEta();
0102 
0103   int quality = 0;
0104   if (useNewQualityCalculation_) {
0105     int r = rank(track);
0106     if (r < 192)
0107       quality = 12;
0108     else if (r < 204)
0109       quality = 13;
0110     else if (r < 220)
0111       quality = 14;
0112     else
0113       quality = 15;
0114   } else {
0115     quality = 12 | (rank(track) >> 6);
0116   }
0117 
0118   int dxy = abs(track.dxy()) >> 8;
0119   if (dxy > 3)
0120     dxy = 3;
0121 
0122   int trackAddr;
0123   std::map<int, int> addr = trackAddress(track, trackAddr);
0124 
0125   l1t::RegionalMuonCand muon(pt, phi, eta, sign, signValid, quality, processor, l1t::bmtf, addr);
0126   muon.setHwHF(HF);
0127   muon.setHwPtUnconstrained(pt2);
0128   muon.setHwDXY(dxy);
0129 
0130   //nw the words!
0131   uint32_t word1 = pt;
0132   word1 = word1 | quality << 9;
0133   word1 = word1 | (twosCompToBits(eta)) << 13;
0134   word1 = word1 | HF << 22;
0135   word1 = word1 | (twosCompToBits(phi)) << 23;
0136 
0137   uint32_t word2 = sign;
0138   word2 = word2 | signValid << 1;
0139   word2 = word2 | dxy << 2;
0140   word2 = word2 | trackAddr << 4;
0141   word2 = word2 | (twosCompToBits(track.wheel())) << 20;
0142   word2 = word2 | pt2 << 23;
0143   muon.setDataword(word2, word1);
0144   return muon;
0145 }
0146 
0147 void L1TMuonBarrelKalmanAlgo::addBMTFMuon(int bx,
0148                                           const L1MuKBMTrack& track,
0149                                           std::unique_ptr<l1t::RegionalMuonCandBxCollection>& out) {
0150   out->push_back(bx, convertToBMTF(track));
0151 }
0152 
0153 // std::pair<bool,uint> L1TMuonBarrelKalmanAlgo::match(const L1MuKBMTCombinedStubRef& seed, const L1MuKBMTCombinedStubRefVector& stubs,int step) {
0154 //   L1MuKBMTCombinedStubRefVector selected;
0155 
0156 //   bool found=false;
0157 //   uint best=0;
0158 //   int distance=100000;
0159 //   uint N=0;
0160 //   for (const auto& stub :stubs)  {
0161 //     N=N+1;
0162 //     if (stub->stNum()!=step)
0163 //       continue;
0164 
0165 //     int d = fabs(wrapAround(((correctedPhi(seed,seed->scNum())-correctedPhi(stub,seed->scNum()))>>3),1024));
0166 //     if (d<distance) {
0167 //       distance = d;
0168 //       best=N-1;
0169 //       found=true;
0170 //     }
0171 //   }
0172 //   return std::make_pair(found,best);
0173 // }
0174 
0175 uint L1TMuonBarrelKalmanAlgo::matchAbs(std::map<uint, uint>& info, uint i, uint j) {
0176   if (info[i] < info[j])
0177     return i;
0178   else
0179     return j;
0180 }
0181 
0182 std::pair<bool, uint> L1TMuonBarrelKalmanAlgo::match(const L1MuKBMTCombinedStubRef& seed,
0183                                                      const L1MuKBMTCombinedStubRefVector& stubs,
0184                                                      int step) {
0185   L1MuKBMTCombinedStubRefVector selected;
0186 
0187   std::map<uint, uint> diffInfo;
0188   for (uint i = 0; i < 12; ++i) {
0189     diffInfo[i] = 60000;
0190   }
0191 
0192   std::map<uint, uint> stubInfo;
0193 
0194   int sector = seed->scNum();
0195   int previousSector = sector - 1;
0196   int nextSector = sector + 1;
0197   if (sector == 0) {
0198     previousSector = 11;
0199   }
0200   if (sector == 11) {
0201     nextSector = 0;
0202   }
0203 
0204   int wheel = seed->whNum();
0205   int innerWheel = 0;
0206   if (wheel == -2)
0207     innerWheel = -1;
0208   if (wheel == -1)
0209     innerWheel = 0;
0210   if (wheel == 0)
0211     innerWheel = 1982;
0212   if (wheel == 1)
0213     innerWheel = 0;
0214   if (wheel == 2)
0215     innerWheel = 1;
0216 
0217   //First align the data
0218   uint N = 0;
0219   for (const auto& stub : stubs) {
0220     N = N + 1;
0221 
0222     if (stub->stNum() != step)
0223       continue;
0224 
0225     uint distance =
0226         fabs(wrapAround(((correctedPhi(seed, seed->scNum()) - correctedPhi(stub, seed->scNum())) >> 3), 1024));
0227 
0228     if (stub->scNum() == previousSector) {
0229       if (stub->whNum() == wheel) {
0230         if (!stub->tag()) {
0231           diffInfo[0] = distance;
0232           stubInfo[0] = N - 1;
0233         } else {
0234           diffInfo[1] = distance;
0235           stubInfo[1] = N - 1;
0236         }
0237       } else if (stub->whNum() == innerWheel) {
0238         if (!stub->tag()) {
0239           diffInfo[2] = distance;
0240           stubInfo[2] = N - 1;
0241         } else {
0242           diffInfo[3] = distance;
0243           stubInfo[3] = N - 1;
0244         }
0245       }
0246     } else if (stub->scNum() == sector) {
0247       if (stub->whNum() == wheel) {
0248         if (!stub->tag()) {
0249           diffInfo[4] = distance;
0250           stubInfo[4] = N - 1;
0251         } else {
0252           diffInfo[5] = distance;
0253           stubInfo[5] = N - 1;
0254         }
0255       } else if (stub->whNum() == innerWheel) {
0256         if (!stub->tag()) {
0257           diffInfo[6] = distance;
0258           stubInfo[6] = N - 1;
0259         } else {
0260           diffInfo[7] = distance;
0261           stubInfo[7] = N - 1;
0262         }
0263       }
0264     } else if (stub->scNum() == nextSector) {
0265       if (stub->whNum() == wheel) {
0266         if (!stub->tag()) {
0267           diffInfo[8] = distance;
0268           stubInfo[8] = N - 1;
0269         } else {
0270           diffInfo[9] = distance;
0271           stubInfo[9] = N - 1;
0272         }
0273       } else if (stub->whNum() == innerWheel) {
0274         if (!stub->tag()) {
0275           diffInfo[10] = distance;
0276           stubInfo[10] = N - 1;
0277         } else {
0278           diffInfo[11] = distance;
0279           stubInfo[11] = N - 1;
0280         }
0281       }
0282     }
0283   }
0284 
0285   uint s1_1 = matchAbs(diffInfo, 0, 1);
0286   uint s1_2 = matchAbs(diffInfo, 2, 3);
0287   uint s1_3 = matchAbs(diffInfo, 4, 5);
0288   uint s1_4 = matchAbs(diffInfo, 6, 7);
0289   uint s1_5 = matchAbs(diffInfo, 8, 9);
0290   uint s1_6 = matchAbs(diffInfo, 10, 11);
0291 
0292   uint s2_1 = matchAbs(diffInfo, s1_1, s1_2);
0293   uint s2_2 = matchAbs(diffInfo, s1_3, s1_4);
0294   uint s2_3 = matchAbs(diffInfo, s1_5, s1_6);
0295 
0296   uint s3_1 = matchAbs(diffInfo, s2_1, s2_2);
0297 
0298   uint s4 = matchAbs(diffInfo, s3_1, s2_3);
0299 
0300   if (diffInfo[s4] != 60000)
0301     return std::make_pair(true, stubInfo[s4]);
0302   else
0303     return std::make_pair(false, 0);
0304 }
0305 
0306 int L1TMuonBarrelKalmanAlgo::correctedPhiB(const L1MuKBMTCombinedStubRef& stub) {
0307   //Promote phiB to 12 bits
0308   return 8 * stub->phiB();
0309 }
0310 
0311 int L1TMuonBarrelKalmanAlgo::correctedPhi(const L1MuKBMTCombinedStubRef& stub, int sector) {
0312   if (stub->scNum() == sector) {
0313     return stub->phi();
0314   } else if ((stub->scNum() == sector - 1) || (stub->scNum() == 11 && sector == 0)) {
0315     return stub->phi() - 2144;
0316   } else if ((stub->scNum() == sector + 1) || (stub->scNum() == 0 && sector == 11)) {
0317     return stub->phi() + 2144;
0318   }
0319   return stub->phi();
0320 }
0321 
0322 int L1TMuonBarrelKalmanAlgo::hitPattern(const L1MuKBMTrack& track) {
0323   unsigned int mask = 0;
0324   for (const auto& stub : track.stubs()) {
0325     mask = mask + round(pow(2, stub->stNum() - 1));
0326   }
0327   return mask;
0328 }
0329 
0330 int L1TMuonBarrelKalmanAlgo::customBitmask(unsigned int bit1, unsigned int bit2, unsigned int bit3, unsigned int bit4) {
0331   return bit1 * 1 + bit2 * 2 + bit3 * 4 + bit4 * 8;
0332 }
0333 
0334 bool L1TMuonBarrelKalmanAlgo::getBit(int bitmask, int pos) { return (bitmask & (1 << pos)) >> pos; }
0335 
0336 void L1TMuonBarrelKalmanAlgo::propagate(L1MuKBMTrack& track) {
0337   int K = track.curvature();
0338   int phi = track.positionAngle();
0339   int phiB = track.bendingAngle();
0340   unsigned int step = track.step();
0341 
0342   //energy loss term only for MU->VERTEX
0343   //int offset=int(charge*eLoss_[step-1]*K*K);
0344   //  if (fabs(offset)>4096)
0345   //      offset=4096*offset/fabs(offset);
0346   int charge = 1;
0347   if (K != 0)
0348     charge = K / fabs(K);
0349 
0350   int KBound = K;
0351 
0352   if (KBound > 4095)
0353     KBound = 4095;
0354   if (KBound < -4095)
0355     KBound = -4095;
0356 
0357   int deltaK = 0;
0358   int KNew = 0;
0359   if (step == 1) {
0360     int addr = KBound / 2;
0361     if (addr < 0)
0362       addr = (-KBound) / 2;
0363     deltaK = 2 * addr - int(2 * addr / (1 + eLoss_[step - 1] * addr));
0364 
0365     if (verbose_)
0366       printf("propagate to vertex K=%d deltaK=%d addr=%d\n", K, deltaK, addr);
0367   }
0368 
0369   if (K >= 0)
0370     KNew = K - deltaK;
0371   else
0372     KNew = K + deltaK;
0373 
0374   //phi propagation
0375   ap_fixed<BITSCURV, BITSCURV> phi11 = ap_fixed<BITSPARAM + 1, 2>(aPhi_[step - 1]) * ap_fixed<BITSCURV, BITSCURV>(K);
0376   ap_fixed<BITSPHIB, BITSPHIB> phi12 =
0377       ap_fixed<BITSPARAM + 1, 2>(-bPhi_[step - 1]) * ap_fixed<BITSPHIB, BITSPHIB>(phiB);
0378 
0379   if (verbose_) {
0380     printf("phi prop = %d * %f = %d, %d * %f = %d\n",
0381            K,
0382            ap_fixed<BITSPARAM + 1, 2>(aPhi_[step - 1]).to_float(),
0383            phi11.to_int(),
0384            phiB,
0385            ap_fixed<BITSPARAM + 1, 2>(-bPhi_[step - 1]).to_float(),
0386            phi12.to_int());
0387   }
0388   int phiNew = ap_fixed<BITSPHI, BITSPHI>(phi + phi11 + phi12);
0389 
0390   //phiB propagation
0391   ap_fixed<BITSCURV, BITSCURV> phiB11 = ap_fixed<BITSPARAM, 1>(aPhiB_[step - 1]) * ap_fixed<BITSCURV, BITSCURV>(K);
0392   ap_fixed<BITSPHIB + 1, BITSPHIB + 1> phiB12 =
0393       ap_ufixed<BITSPARAM + 1, 1>(bPhiB_[step - 1]) * ap_fixed<BITSPHIB, BITSPHIB>(phiB);
0394   int phiBNew = ap_fixed<13, 13>(phiB11 + phiB12);
0395   if (verbose_) {
0396     printf("phiB prop = %d * %f = %d, %d * %f = %d\n",
0397            K,
0398            ap_fixed<BITSPARAM + 1, 2>(aPhiB_[step - 1]).to_float(),
0399            phiB11.to_int(),
0400            phiB,
0401            ap_ufixed<BITSPARAM + 1, 1>(bPhiB_[step - 1]).to_float(),
0402            phiB12.to_int());
0403   }
0404 
0405   //Only for the propagation to vertex we use the LUT for better precision and the full function
0406   if (step == 1) {
0407     int addr = KBound / 2;
0408     // Extra steps to mimic firmware for vertex prop
0409     ap_ufixed<11, 11> dxyOffset = (int)fabs(aPhiB_[step - 1] * addr / (1 + charge * aPhiBNLO_[step - 1] * addr));
0410     ap_fixed<12, 12> DXY;
0411     if (addr > 0)
0412       DXY = -dxyOffset;
0413     else
0414       DXY = dxyOffset;
0415     phiBNew = ap_fixed<BITSPHIB, BITSPHIB>(DXY - ap_fixed<BITSPHIB, BITSPHIB>(phiB));
0416     if (verbose_) {
0417       printf("Vertex phiB prop = %d - %d = %d\n", DXY.to_int(), ap_fixed<BITSPHIB, BITSPHIB>(phiB).to_int(), phiBNew);
0418     }
0419   }
0420   ///////////////////////////////////////////////////////
0421   //Rest of the stuff  is for the offline version only
0422   //where we want to check what is happening in the covariaznce matrix
0423 
0424   //Create the transformation matrix
0425   double a[9];
0426   a[0] = 1.;
0427   a[1] = 0.0;
0428   a[2] = 0.0;
0429   a[3] = aPhi_[step - 1];
0430   //  a[3] = 0.0;
0431   a[4] = 1.0;
0432   a[5] = -bPhi_[step - 1];
0433   //a[6]=0.0;
0434   a[6] = aPhiB_[step - 1];
0435   if (step == 1)
0436     a[6] = aPhiB_[step - 1] / 2.0;
0437 
0438   a[7] = 0.0;
0439   a[8] = bPhiB_[step - 1];
0440 
0441   ROOT::Math::SMatrix<double, 3> P(a, 9);
0442 
0443   const std::vector<double>& covLine = track.covariance();
0444   L1MuKBMTrack::CovarianceMatrix cov(covLine.begin(), covLine.end());
0445   cov = ROOT::Math::Similarity(P, cov);
0446 
0447   //Add the multiple scattering
0448   double phiRMS = mScatteringPhi_[step - 1] * K * K;
0449   double phiBRMS = mScatteringPhiB_[step - 1] * K * K;
0450 
0451   std::vector<double> b(6);
0452   b[0] = 0;
0453   b[1] = 0;
0454   b[2] = phiRMS;
0455   b[3] = 0;
0456   b[4] = 0;
0457   b[5] = phiBRMS;
0458 
0459   reco::Candidate::CovarianceMatrix MS(b.begin(), b.end());
0460 
0461   cov = cov + MS;
0462 
0463   if (verbose_) {
0464     printf("Covariance term for phiB = %f\n", cov(2, 2));
0465     printf("Multiple scattering term for phiB = %f\n", MS(2, 2));
0466   }
0467 
0468   track.setCovariance(cov);
0469   track.setCoordinates(step - 1, KNew, phiNew, phiBNew);
0470 }
0471 
0472 bool L1TMuonBarrelKalmanAlgo::update(L1MuKBMTrack& track, const L1MuKBMTCombinedStubRef& stub, int mask, int seedQual) {
0473   updateEta(track, stub);
0474   if (useOfflineAlgo_) {
0475     if (mask == 3 || mask == 5 || mask == 9 || mask == 6 || mask == 10 || mask == 12)
0476       return updateOffline(track, stub);
0477     else
0478       return updateOffline1D(track, stub);
0479 
0480   } else
0481     return updateLUT(track, stub, mask, seedQual);
0482 }
0483 
0484 bool L1TMuonBarrelKalmanAlgo::updateOffline(L1MuKBMTrack& track, const L1MuKBMTCombinedStubRef& stub) {
0485   int trackK = track.curvature();
0486   int trackPhi = track.positionAngle();
0487   int trackPhiB = track.bendingAngle();
0488 
0489   int phi = correctedPhi(stub, track.sector());
0490   int phiB = correctedPhiB(stub);
0491 
0492   Vector2 residual;
0493   residual[0] = phi - trackPhi;
0494   residual[1] = phiB - trackPhiB;
0495 
0496   Matrix23 H;
0497   H(0, 0) = 0.0;
0498   H(0, 1) = 1.0;
0499   H(0, 2) = 0.0;
0500   H(1, 0) = 0.0;
0501   H(1, 1) = 0.0;
0502   H(1, 2) = 1.0;
0503 
0504   CovarianceMatrix2 R;
0505   R(0, 0) = pointResolutionPhi_;
0506   R(0, 1) = 0.0;
0507   R(1, 0) = 0.0;
0508   if (stub->quality() < 4)
0509     R(1, 1) = pointResolutionPhiBL_[track.step() - 1];
0510   else
0511     R(1, 1) = pointResolutionPhiBH_[track.step() - 1];
0512 
0513   const std::vector<double>& covLine = track.covariance();
0514   L1MuKBMTrack::CovarianceMatrix cov(covLine.begin(), covLine.end());
0515 
0516   CovarianceMatrix2 S = ROOT::Math::Similarity(H, cov) + R;
0517   if (!S.Invert())
0518     return false;
0519   Matrix32 Gain = cov * ROOT::Math::Transpose(H) * S;
0520 
0521   track.setKalmanGain(
0522       track.step(), fabs(trackK), Gain(0, 0), Gain(0, 1), Gain(1, 0), Gain(1, 1), Gain(2, 0), Gain(2, 1));
0523 
0524   int KNew = (trackK + int(Gain(0, 0) * residual(0) + Gain(0, 1) * residual(1)));
0525   if (fabs(KNew) > 8192)
0526     return false;
0527 
0528   int phiNew = wrapAround(trackPhi + residual(0), 8192);
0529   int phiBNew = wrapAround(trackPhiB + int(Gain(2, 0) * residual(0) + Gain(2, 1) * residual(1)), 4096);
0530 
0531   track.setResidual(stub->stNum() - 1, fabs(phi - phiNew) + fabs(phiB - phiBNew) / 8);
0532 
0533   if (verbose_) {
0534     printf("residual %d - %d = %d %d - %d = %d\n", phi, trackPhi, int(residual[0]), phiB, trackPhiB, int(residual[1]));
0535     printf("Gains offline: %f %f %f %f\n", Gain(0, 0), Gain(0, 1), Gain(2, 0), Gain(2, 1));
0536     printf(" K = %d + %f * %f + %f * %f\n", trackK, Gain(0, 0), residual(0), Gain(0, 1), residual(1));
0537     printf(" phiB = %d + %f * %f + %f * %f\n", trackPhiB, Gain(2, 0), residual(0), Gain(2, 1), residual(1));
0538   }
0539 
0540   track.setCoordinates(track.step(), KNew, phiNew, phiBNew);
0541   Matrix33 covNew = cov - Gain * (H * cov);
0542   L1MuKBMTrack::CovarianceMatrix c;
0543 
0544   c(0, 0) = covNew(0, 0);
0545   c(0, 1) = covNew(0, 1);
0546   c(0, 2) = covNew(0, 2);
0547   c(1, 0) = covNew(1, 0);
0548   c(1, 1) = covNew(1, 1);
0549   c(1, 2) = covNew(1, 2);
0550   c(2, 0) = covNew(2, 0);
0551   c(2, 1) = covNew(2, 1);
0552   c(2, 2) = covNew(2, 2);
0553   if (verbose_) {
0554     printf("Post Fit Covariance Matrix %f %f %f\n", cov(0, 0), cov(1, 1), cov(2, 2));
0555   }
0556 
0557   track.setCovariance(c);
0558   track.addStub(stub);
0559   track.setHitPattern(hitPattern(track));
0560 
0561   return true;
0562 }
0563 
0564 bool L1TMuonBarrelKalmanAlgo::updateOffline1D(L1MuKBMTrack& track, const L1MuKBMTCombinedStubRef& stub) {
0565   int trackK = track.curvature();
0566   int trackPhi = track.positionAngle();
0567   int trackPhiB = track.bendingAngle();
0568 
0569   int phi = correctedPhi(stub, track.sector());
0570 
0571   double residual = phi - trackPhi;
0572 
0573   if (verbose_)
0574     printf("residuals %d - %d = %d\n", phi, trackPhi, int(residual));
0575 
0576   Matrix13 H;
0577   H(0, 0) = 0.0;
0578   H(0, 1) = 1.0;
0579   H(0, 2) = 0.0;
0580 
0581   const std::vector<double>& covLine = track.covariance();
0582   L1MuKBMTrack::CovarianceMatrix cov(covLine.begin(), covLine.end());
0583 
0584   double S = ROOT::Math::Similarity(H, cov)(0, 0) + pointResolutionPhi_;
0585 
0586   if (S == 0.0)
0587     return false;
0588   Matrix31 Gain = cov * ROOT::Math::Transpose(H) / S;
0589 
0590   track.setKalmanGain(track.step(), fabs(trackK), Gain(0, 0), 0.0, Gain(1, 0), 0.0, Gain(2, 0), 0.0);
0591   if (verbose_)
0592     printf("Gains: %f %f\n", Gain(0, 0), Gain(2, 0));
0593 
0594   int KNew = wrapAround(trackK + int(Gain(0, 0) * residual), 8192);
0595   int phiNew = wrapAround(trackPhi + residual, 8192);
0596   int phiBNew = wrapAround(trackPhiB + int(Gain(2, 0) * residual), 4096);
0597   track.setCoordinates(track.step(), KNew, phiNew, phiBNew);
0598   Matrix33 covNew = cov - Gain * (H * cov);
0599   L1MuKBMTrack::CovarianceMatrix c;
0600 
0601   if (verbose_) {
0602     printf("phiUpdate: %d %d\n", int(Gain(0, 0) * residual), int(Gain(2, 0) * residual));
0603   }
0604 
0605   c(0, 0) = covNew(0, 0);
0606   c(0, 1) = covNew(0, 1);
0607   c(0, 2) = covNew(0, 2);
0608   c(1, 0) = covNew(1, 0);
0609   c(1, 1) = covNew(1, 1);
0610   c(1, 2) = covNew(1, 2);
0611   c(2, 0) = covNew(2, 0);
0612   c(2, 1) = covNew(2, 1);
0613   c(2, 2) = covNew(2, 2);
0614   track.setCovariance(c);
0615   track.addStub(stub);
0616   track.setHitPattern(hitPattern(track));
0617 
0618   return true;
0619 }
0620 
0621 bool L1TMuonBarrelKalmanAlgo::updateLUT(L1MuKBMTrack& track,
0622                                         const L1MuKBMTCombinedStubRef& stub,
0623                                         int mask,
0624                                         int seedQual) {
0625   int trackK = track.curvature();
0626   int trackPhi = track.positionAngle();
0627   int trackPhiB = track.bendingAngle();
0628 
0629   int phi = correctedPhi(stub, track.sector());
0630   int phiB = correctedPhiB(stub);
0631 
0632   Vector2 residual;
0633   ap_fixed<BITSPHI + 1, BITSPHI + 1> residualPhi = phi - trackPhi;
0634   ap_fixed<BITSPHIB + 1, BITSPHIB + 1> residualPhiB = phiB - trackPhiB;
0635 
0636   if (verbose_)
0637     printf("residual %d - %d = %d %d - %d = %d\n",
0638            phi,
0639            trackPhi,
0640            residualPhi.to_int(),
0641            phiB,
0642            trackPhiB,
0643            residualPhiB.to_int());
0644 
0645   uint absK = fabs(trackK);
0646   if (absK > 4095)
0647     absK = 4095;
0648 
0649   std::vector<float> GAIN;
0650   //For the three stub stuff use only gains 0 and 4
0651   if (!(mask == 3 || mask == 5 || mask == 9 || mask == 6 || mask == 10 || mask == 12)) {
0652     GAIN = lutService_->trackGain(track.step(), track.hitPattern(), absK / 4);
0653     GAIN[1] = 0.0;
0654     GAIN[3] = 0.0;
0655 
0656   } else {
0657     GAIN = lutService_->trackGain2(track.step(), track.hitPattern(), absK / 8, seedQual, stub->quality());
0658   }
0659   if (verbose_) {
0660     printf("Gains (fp): %f %f %f %f\n", GAIN[0], GAIN[1], GAIN[2], GAIN[3]);
0661     if (!(mask == 3 || mask == 5 || mask == 9 || mask == 6 || mask == 10 || mask == 12))
0662       printf("Addr=%d   gain0=%f gain4=-%f\n",
0663              absK / 4,
0664              ap_ufixed<GAIN_0, GAIN_0INT>(GAIN[0]).to_float(),
0665              ap_ufixed<GAIN_4, GAIN_4INT>(GAIN[2]).to_float());
0666     else
0667       printf("Addr=%d   %f -%f %f %f\n",
0668              absK / 4,
0669              ap_fixed<GAIN2_0, GAIN2_0INT>(GAIN[0]).to_float(),
0670              ap_ufixed<GAIN2_1, GAIN2_1INT>(GAIN[1]).to_float(),
0671              ap_ufixed<GAIN2_4, GAIN2_4INT>(GAIN[2]).to_float(),
0672              ap_ufixed<GAIN2_5, GAIN2_5INT>(GAIN[3]).to_float());
0673   }
0674 
0675   track.setKalmanGain(track.step(), fabs(trackK), GAIN[0], GAIN[1], 1, 0, GAIN[2], GAIN[3]);
0676 
0677   int KNew;
0678   if (!(mask == 3 || mask == 5 || mask == 9 || mask == 6 || mask == 10 || mask == 12)) {
0679     KNew = ap_fixed<BITSPHI + 9, BITSPHI + 9>(ap_fixed<BITSCURV, BITSCURV>(trackK) +
0680                                               ap_ufixed<GAIN_0, GAIN_0INT>(GAIN[0]) * residualPhi);
0681   } else {
0682     ap_fixed<BITSPHI + 7, BITSPHI + 7> k11 = ap_fixed<GAIN2_0, GAIN2_0INT>(GAIN[0]) * residualPhi;
0683     ap_fixed<BITSPHIB + 4, BITSPHIB + 4> k12 = ap_ufixed<GAIN2_1, GAIN2_1INT>(GAIN[1]) * residualPhiB;
0684     KNew = ap_fixed<BITSPHI + 9, BITSPHI + 9>(ap_fixed<BITSCURV, BITSCURV>(trackK) + k11 - k12);
0685   }
0686   if (fabs(KNew) >= 8191)
0687     return false;
0688   KNew = wrapAround(KNew, 8192);
0689   int phiNew = phi;
0690 
0691   //different products for different firmware logic
0692   ap_fixed<BITSPHI + 5, BITSPHI + 5> pbdouble_0 = ap_ufixed<GAIN2_4, GAIN2_4INT>(GAIN[2]) * residualPhi;
0693   ap_fixed<BITSPHIB + 24, BITSPHIB + 4> pb_1 = ap_ufixed<GAIN2_5, GAIN2_5INT>(GAIN[3]) * residualPhiB;
0694   ap_fixed<BITSPHI + 9, BITSPHI + 5> pb_0 = ap_ufixed<GAIN_4, GAIN_4INT>(GAIN[2]) * residualPhi;
0695 
0696   if (verbose_) {
0697     printf("phiupdate %f %f %f\n", pb_0.to_float(), pb_1.to_float(), pbdouble_0.to_float());
0698   }
0699 
0700   int phiBNew;
0701   if (!(mask == 3 || mask == 5 || mask == 9 || mask == 6 || mask == 10 || mask == 12)) {
0702     phiBNew = ap_fixed<BITSPHI + 8, BITSPHI + 8>(ap_fixed<BITSPHIB, BITSPHIB>(trackPhiB) -
0703                                                  ap_ufixed<GAIN_4, GAIN_4INT>(GAIN[2]) * residualPhi);
0704 
0705     if (fabs(phiBNew) >= 4095)
0706       return false;
0707   } else {
0708     phiBNew = ap_fixed<BITSPHI + 7, BITSPHI + 7>(ap_fixed<BITSPHIB, BITSPHIB>(trackPhiB) + pb_1 - pbdouble_0);
0709     if (fabs(phiBNew) >= 4095)
0710       return false;
0711   }
0712   track.setCoordinates(track.step(), KNew, phiNew, phiBNew);
0713   track.addStub(stub);
0714   track.setHitPattern(hitPattern(track));
0715   return true;
0716 }
0717 
0718 void L1TMuonBarrelKalmanAlgo::updateEta(L1MuKBMTrack& track, const L1MuKBMTCombinedStubRef& stub) {}
0719 
0720 void L1TMuonBarrelKalmanAlgo::vertexConstraint(L1MuKBMTrack& track) {
0721   if (useOfflineAlgo_)
0722     vertexConstraintOffline(track);
0723   else
0724     vertexConstraintLUT(track);
0725 }
0726 
0727 void L1TMuonBarrelKalmanAlgo::vertexConstraintOffline(L1MuKBMTrack& track) {
0728   double residual = -track.dxy();
0729   Matrix13 H;
0730   H(0, 0) = 0;
0731   H(0, 1) = 0;
0732   H(0, 2) = 1;
0733 
0734   const std::vector<double>& covLine = track.covariance();
0735   L1MuKBMTrack::CovarianceMatrix cov(covLine.begin(), covLine.end());
0736 
0737   double S = (ROOT::Math::Similarity(H, cov))(0, 0) + pointResolutionVertex_;
0738   S = 1.0 / S;
0739   Matrix31 Gain = cov * (ROOT::Math::Transpose(H)) * S;
0740   track.setKalmanGain(track.step(), fabs(track.curvature()), Gain(0, 0), Gain(1, 0), Gain(2, 0));
0741 
0742   if (verbose_) {
0743     printf("sigma3=%f sigma6=%f\n", cov(0, 3), cov(3, 3));
0744     printf(" K = %d + %f * %f\n", track.curvature(), Gain(0, 0), residual);
0745   }
0746 
0747   int KNew = wrapAround(int(track.curvature() + Gain(0, 0) * residual), 8192);
0748   int phiNew = wrapAround(int(track.positionAngle() + Gain(1, 0) * residual), 8192);
0749   int dxyNew = wrapAround(int(track.dxy() + Gain(2, 0) * residual), 8192);
0750   if (verbose_)
0751     printf("Post fit impact parameter=%d\n", dxyNew);
0752   track.setCoordinatesAtVertex(KNew, phiNew, -residual);
0753   Matrix33 covNew = cov - Gain * (H * cov);
0754   L1MuKBMTrack::CovarianceMatrix c;
0755   c(0, 0) = covNew(0, 0);
0756   c(0, 1) = covNew(0, 1);
0757   c(0, 2) = covNew(0, 2);
0758   c(1, 0) = covNew(1, 0);
0759   c(1, 1) = covNew(1, 1);
0760   c(1, 2) = covNew(1, 2);
0761   c(2, 0) = covNew(2, 0);
0762   c(2, 1) = covNew(2, 1);
0763   c(2, 2) = covNew(2, 2);
0764   track.setCovariance(c);
0765   //  track.covariance = track.covariance - Gain*H*track.covariance;
0766 }
0767 
0768 void L1TMuonBarrelKalmanAlgo::vertexConstraintLUT(L1MuKBMTrack& track) {
0769   double residual = -track.dxy();
0770   uint absK = fabs(track.curvature());
0771   if (absK > 2047)
0772     absK = 2047;
0773 
0774   std::pair<float, float> GAIN = lutService_->vertexGain(track.hitPattern(), absK / 2);
0775   track.setKalmanGain(track.step(), fabs(track.curvature()), GAIN.first, GAIN.second, -1);
0776 
0777   ap_fixed<BITSCURV, BITSCURV> k_0 =
0778       -(ap_ufixed<GAIN_V0, GAIN_V0INT>(fabs(GAIN.first))) * ap_fixed<BITSPHIB, BITSPHIB>(residual);
0779   int KNew = ap_fixed<BITSCURV, BITSCURV>(k_0 + ap_fixed<BITSCURV, BITSCURV>(track.curvature()));
0780 
0781   if (verbose_) {
0782     printf("VERTEX GAIN(%d)= -%f * %d = %d\n",
0783            absK / 2,
0784            ap_ufixed<GAIN_V0, GAIN_V0INT>(fabs(GAIN.first)).to_float(),
0785            ap_fixed<BITSPHIB, BITSPHIB>(residual).to_int(),
0786            k_0.to_int());
0787   }
0788 
0789   int p_0 = fp_product(GAIN.second, int(residual), 7);
0790   int phiNew = wrapAround(track.positionAngle() + p_0, 8192);
0791   track.setCoordinatesAtVertex(KNew, phiNew, -residual);
0792 }
0793 
0794 void L1TMuonBarrelKalmanAlgo::setFloatingPointValues(L1MuKBMTrack& track, bool vertex) {
0795   int K, etaINT;
0796 
0797   if (track.hasFineEta())
0798     etaINT = track.fineEta();
0799   else
0800     etaINT = track.coarseEta();
0801 
0802   double lsb = 1.25 / float(1 << 13);
0803   double lsbEta = 0.010875;
0804 
0805   if (vertex) {
0806     int charge = 1;
0807     if (track.curvatureAtVertex() < 0)
0808       charge = -1;
0809     double pt = double(ptLUT(track.curvatureAtVertex())) / 2.0;
0810 
0811     double phi = track.sector() * M_PI / 6.0 + track.phiAtVertex() * M_PI / (6 * 2048.) - 2 * M_PI;
0812 
0813     double eta = etaINT * lsbEta;
0814     track.setPtEtaPhi(pt, eta, phi);
0815     track.setCharge(charge);
0816   } else {
0817     K = track.curvatureAtMuon();
0818     if (K == 0)
0819       K = 1;
0820 
0821     if (fabs(K) < 46)
0822       K = 46 * K / fabs(K);
0823     double pt = 1.0 / (lsb * fabs(K));
0824     if (pt < 1.6)
0825       pt = 1.6;
0826     track.setPtUnconstrained(pt);
0827   }
0828 }
0829 
0830 std::pair<bool, L1MuKBMTrack> L1TMuonBarrelKalmanAlgo::chain(const L1MuKBMTCombinedStubRef& seed,
0831                                                              const L1MuKBMTCombinedStubRefVector& stubs) {
0832   L1MuKBMTrackCollection pretracks;
0833   std::vector<int> combinatorics;
0834   int seedQual;
0835   switch (seed->stNum()) {
0836     case 1:
0837       combinatorics = combos1_;
0838       break;
0839     case 2:
0840       combinatorics = combos2_;
0841       break;
0842 
0843     case 3:
0844       combinatorics = combos3_;
0845       break;
0846 
0847     case 4:
0848       combinatorics = combos4_;
0849       break;
0850 
0851     default:
0852       printf("Something really bad happend\n");
0853   }
0854 
0855   L1MuKBMTrack nullTrack(seed, correctedPhi(seed, seed->scNum()), correctedPhiB(seed));
0856   seedQual = seed->quality();
0857   for (const auto& mask : combinatorics) {
0858     L1MuKBMTrack track(seed, correctedPhi(seed, seed->scNum()), correctedPhiB(seed));
0859     int phiB = correctedPhiB(seed);
0860     int charge;
0861     if (phiB == 0)
0862       charge = 0;
0863     else
0864       charge = phiB / fabs(phiB);
0865 
0866     int address = phiB;
0867     if (track.step() == 4 && (fabs(seed->phiB()) > 15))
0868       address = charge * 15 * 8;
0869 
0870     if (track.step() == 3 && (fabs(seed->phiB()) > 30))
0871       address = charge * 30 * 8;
0872     if (track.step() == 2 && (fabs(seed->phiB()) > 127))
0873       address = charge * 127 * 8;
0874     int initialK = int(initK_[seed->stNum() - 1] * address / (1 + initK2_[seed->stNum() - 1] * charge * address));
0875     if (initialK > 8191)
0876       initialK = 8191;
0877     if (initialK < -8191)
0878       initialK = -8191;
0879 
0880     track.setCoordinates(seed->stNum(), initialK, correctedPhi(seed, seed->scNum()), phiB);
0881     if (seed->quality() < 4) {
0882       track.setCoordinates(seed->stNum(), 0, correctedPhi(seed, seed->scNum()), 0);
0883     }
0884 
0885     track.setHitPattern(hitPattern(track));
0886     //set covariance
0887     L1MuKBMTrack::CovarianceMatrix covariance;
0888 
0889     float DK = 512 * 512.;
0890     covariance(0, 0) = DK;
0891     covariance(0, 1) = 0;
0892     covariance(0, 2) = 0;
0893     covariance(1, 0) = 0;
0894     covariance(1, 1) = float(pointResolutionPhi_);
0895     covariance(1, 2) = 0;
0896     covariance(2, 0) = 0;
0897     covariance(2, 1) = 0;
0898     if (!(mask == 3 || mask == 5 || mask == 9 || mask == 6 || mask == 10 || mask == 12))
0899       covariance(2, 2) = float(pointResolutionPhiB_);
0900     else {
0901       if (seed->quality() < 4)
0902         covariance(2, 2) = float(pointResolutionPhiBL_[seed->stNum() - 1]);
0903       else
0904         covariance(2, 2) = float(pointResolutionPhiBH_[seed->stNum() - 1]);
0905     }
0906     track.setCovariance(covariance);
0907 
0908     //
0909     if (verbose_) {
0910       printf("New Kalman fit staring at step=%d, phi=%d,phiB=%d with curvature=%d\n",
0911              track.step(),
0912              track.positionAngle(),
0913              track.bendingAngle(),
0914              track.curvature());
0915       printf("BITMASK:");
0916       for (unsigned int i = 0; i < 4; ++i)
0917         printf("%d", getBit(mask, i));
0918       printf("\n");
0919       printf("------------------------------------------------------\n");
0920       printf("------------------------------------------------------\n");
0921       printf("------------------------------------------------------\n");
0922       printf("stubs:\n");
0923       for (const auto& stub : stubs)
0924         printf("station=%d phi=%d phiB=%d qual=%d tag=%d sector=%d wheel=%d fineEta= %d %d\n",
0925                stub->stNum(),
0926                correctedPhi(stub, seed->scNum()),
0927                correctedPhiB(stub),
0928                stub->quality(),
0929                stub->tag(),
0930                stub->scNum(),
0931                stub->whNum(),
0932                stub->eta1(),
0933                stub->eta2());
0934       printf("------------------------------------------------------\n");
0935       printf("------------------------------------------------------\n");
0936     }
0937 
0938     int phiAtStation2 = 0;
0939 
0940     while (track.step() > 0) {
0941       // muon station 1
0942       if (track.step() == 1) {
0943         track.setCoordinatesAtMuon(track.curvature(), track.positionAngle(), track.bendingAngle());
0944         phiAtStation2 = phiAt2(track);
0945         bool passed = estimateChiSquare(track);
0946         if (!passed)
0947           break;
0948         calculateEta(track);
0949         setFloatingPointValues(track, false);
0950         //calculate coarse eta
0951         //////////////////////
0952 
0953         if (verbose_)
0954           printf("Unconstrained PT  in Muon System: pt=%f\n", track.ptUnconstrained());
0955       }
0956 
0957       propagate(track);
0958       if (verbose_)
0959         printf("propagated Coordinates step:%d,phi=%d,phiB=%d,K=%d\n",
0960                track.step(),
0961                track.positionAngle(),
0962                track.bendingAngle(),
0963                track.curvature());
0964 
0965       if (track.step() > 0)
0966         if (getBit(mask, track.step() - 1)) {
0967           std::pair<bool, uint> bestStub = match(seed, stubs, track.step());
0968           if ((!bestStub.first) || (!update(track, stubs[bestStub.second], mask, seedQual)))
0969             break;
0970           if (verbose_) {
0971             printf("updated Coordinates step:%d,phi=%d,phiB=%d,K=%d\n",
0972                    track.step(),
0973                    track.positionAngle(),
0974                    track.bendingAngle(),
0975                    track.curvature());
0976           }
0977         }
0978 
0979       if (track.step() == 0) {
0980         track.setCoordinatesAtVertex(track.curvature(), track.positionAngle(), track.bendingAngle());
0981         if (verbose_)
0982           printf(" Coordinates before vertex constraint step:%d,phi=%d,dxy=%d,K=%d\n",
0983                  track.step(),
0984                  track.phiAtVertex(),
0985                  track.dxy(),
0986                  track.curvatureAtVertex());
0987         if (verbose_)
0988           printf("Chi Square = %d\n", track.approxChi2());
0989 
0990         vertexConstraint(track);
0991         estimateCompatibility(track);
0992         if (verbose_) {
0993           printf(" Coordinates after vertex constraint step:%d,phi=%d,dxy=%d,K=%d  maximum local chi2=%d\n",
0994                  track.step(),
0995                  track.phiAtVertex(),
0996                  track.dxy(),
0997                  track.curvatureAtVertex(),
0998                  track.approxChi2());
0999           printf("------------------------------------------------------\n");
1000           printf("------------------------------------------------------\n");
1001         }
1002         setFloatingPointValues(track, true);
1003         //set the coordinates at muon to include phi at station 2
1004         track.setCoordinatesAtMuon(track.curvatureAtMuon(), phiAtStation2, track.phiBAtMuon());
1005         track.setRank(rank(track));
1006         if (verbose_)
1007           printf("Floating point coordinates at vertex: pt=%f, eta=%f phi=%f\n", track.pt(), track.eta(), track.phi());
1008         pretracks.push_back(track);
1009       }
1010     }
1011   }
1012 
1013   //Now for all the pretracks we need only one
1014   L1MuKBMTrackCollection cleaned = clean(pretracks, seed->stNum());
1015 
1016   if (!cleaned.empty()) {
1017     return std::make_pair(true, cleaned[0]);
1018   }
1019   return std::make_pair(false, nullTrack);
1020 }
1021 
1022 bool L1TMuonBarrelKalmanAlgo::estimateChiSquare(L1MuKBMTrack& track) {
1023   //here we have a simplification of the algorithm for the sake of the emulator - rsult is identical
1024   // we apply cuts on the firmware as |u -u'|^2 < a+b *K^2
1025   int K = track.curvatureAtMuon();
1026 
1027   uint chi = 0;
1028 
1029   // const double PHI[4]={0.0,0.249,0.543,0.786};
1030   // const double DROR[4]={0.0,0.182,0.430,0.677};
1031 
1032   int coords = wrapAround((track.phiAtMuon() + track.phiBAtMuon()) >> 4, 512);
1033   for (const auto& stub : track.stubs()) {
1034     int AK = wrapAround(fp_product(-chiSquare_[stub->stNum() - 1], K >> 4, 8), 256);
1035     int stubCoords = wrapAround((correctedPhi(stub, track.sector()) >> 4) + (stub->phiB() >> 1), 512);
1036     int diff1 = wrapAround(stubCoords - coords, 1024);
1037     uint delta = wrapAround(abs(diff1 + AK), 2048);
1038     chi = chi + delta;
1039     if (verbose_)
1040       printf("Chi Square stub for track with pattern=%d coords=%d -> AK=%d stubCoords=%d diff=%d delta=%d\n",
1041              track.hitPattern(),
1042              coords,
1043              AK,
1044              stubCoords,
1045              diff1,
1046              delta);
1047   }
1048 
1049   if (chi > 127)
1050     chi = 127;
1051   // for (const auto& stub: track.stubs()) {
1052   //   int deltaPhi = (correctedPhi(stub,track.sector())-track.phiAtMuon())>>3;
1053   //   int AK =  fp_product(PHI[stub->stNum()-1],K>>3,8);
1054   //   int BPB = fp_product(DROR[stub->stNum()-1],track.phiBAtMuon()>>3,8);
1055   //   chi=chi+abs(deltaPhi-AK-BPB);
1056   // }
1057   // //  }
1058 
1059   track.setApproxChi2(chi);
1060   for (uint i = 0; i < chiSquareCutPattern_.size(); ++i) {
1061     if (track.hitPattern() == chiSquareCutPattern_[i] && fabs(K) < chiSquareCutCurv_[i] &&
1062         track.approxChi2() > chiSquareCut_[i])
1063       return false;
1064   }
1065   return true;
1066 }
1067 
1068 void L1TMuonBarrelKalmanAlgo::estimateCompatibility(L1MuKBMTrack& track) {
1069   int K = track.curvatureAtVertex() >> 4;
1070 
1071   if (track.stubs().size() != 2) {
1072     track.setTrackCompatibility(0);
1073     return;
1074   }
1075 
1076   uint stubSel = 1;
1077   if (track.stubs()[0]->quality() > track.stubs()[1]->quality())
1078     stubSel = 0;
1079   const L1MuKBMTCombinedStubRef& stub = track.stubs()[stubSel];
1080 
1081   if (verbose_) {
1082     printf("stubsel %d phi=%d phiB=%d\n", stubSel, stub->phi(), stub->phiB());
1083   }
1084 
1085   ap_ufixed<BITSCURV - 5, BITSCURV - 5> absK;
1086   if (K < 0)
1087     absK = -K;
1088   else
1089     absK = K;
1090 
1091   ap_fixed<12, 12> diff = ap_int<10>(stub->phiB()) -
1092                           ap_ufixed<5, 1>(trackComp_[stub->stNum() - 1]) * ap_fixed<BITSCURV - 4, BITSCURV - 4>(K);
1093   ap_ufixed<11, 11> delta;
1094   if (diff.is_neg())
1095     delta = -diff;
1096   else
1097     delta = diff;
1098 
1099   ap_ufixed<BITSCURV - 5, BITSCURV - 5> err =
1100       ap_uint<3>(trackCompErr1_[stub->stNum() - 1]) + ap_ufixed<5, 0>(trackCompErr2_[stub->stNum() - 1]) * absK;
1101   track.setTrackCompatibility(((int)delta) / ((int)err));
1102   for (uint i = 0; i < trackCompPattern_.size(); ++i) {
1103     int deltaMax = ap_ufixed<BITSCURV, BITSCURV>(err * trackCompCut_[i]);
1104     if (verbose_) {
1105       if (track.hitPattern() == trackCompPattern_[i]) {
1106         printf("delta = %d = abs(%d - %f*%d\n", delta.to_int(), stub->phiB(), trackComp_[stub->stNum() - 1], K);
1107         printf("err = %d = %f + %f*%d\n",
1108                err.to_int(),
1109                trackCompErr1_[stub->stNum() - 1],
1110                trackCompErr2_[stub->stNum() - 1],
1111                absK.to_int());
1112         printf("deltaMax = %d = %d*%d\n", deltaMax, err.to_int(), trackCompCut_[i]);
1113       }
1114     }
1115     if ((track.hitPattern() == trackCompPattern_[i]) && ((int)absK < trackCompCutCurv_[i]) &&
1116         ((track.approxChi2() > chiSquareCutTight_[i]) || (delta > deltaMax))) {
1117       track.setCoordinatesAtVertex(8191, track.phiAtVertex(), track.dxy());
1118       break;
1119     }
1120   }
1121 }
1122 
1123 int L1TMuonBarrelKalmanAlgo::rank(const L1MuKBMTrack& track) {
1124   //    int offset=0;
1125   uint chi = track.approxChi2() > 127 ? 127 : track.approxChi2();
1126   if (hitPattern(track) == customBitmask(0, 0, 1, 1)) {
1127     return 60;
1128   }
1129   //    return offset+(track.stubs().size()*2+track.quality())*80-track.approxChi2();
1130   return 160 + (track.stubs().size()) * 20 - chi;
1131 }
1132 
1133 int L1TMuonBarrelKalmanAlgo::wrapAround(int value, int maximum) {
1134   if (value > maximum - 1)
1135     return wrapAround(value - 2 * maximum, maximum);
1136   if (value < -maximum)
1137     return wrapAround(value + 2 * maximum, maximum);
1138   return value;
1139 }
1140 
1141 int L1TMuonBarrelKalmanAlgo::encode(bool ownwheel, int sector, bool tag) {
1142   if (ownwheel) {
1143     if (sector == 0) {
1144       if (tag)
1145         return 9;
1146       else
1147         return 8;
1148     } else if (sector == 1) {
1149       if (tag)
1150         return 11;
1151       else
1152         return 10;
1153 
1154     } else {
1155       if (tag)
1156         return 13;
1157       else
1158         return 12;
1159     }
1160 
1161   } else {
1162     if (sector == 0) {
1163       if (tag)
1164         return 1;
1165       else
1166         return 0;
1167     } else if (sector == 1) {
1168       if (tag)
1169         return 3;
1170       else
1171         return 2;
1172 
1173     } else {
1174       if (tag)
1175         return 5;
1176       else
1177         return 4;
1178     }
1179   }
1180   return 15;
1181 }
1182 
1183 std::map<int, int> L1TMuonBarrelKalmanAlgo::trackAddress(const L1MuKBMTrack& track, int& word) {
1184   std::map<int, int> out;
1185 
1186   out[l1t::RegionalMuonCand::kWheelSide] = track.wheel() < 0;
1187   if (track.wheel() == -2)
1188     out[l1t::RegionalMuonCand::kWheelNum] = 2;
1189   else if (track.wheel() == -1)
1190     out[l1t::RegionalMuonCand::kWheelNum] = 1;
1191   else if (track.wheel() == 0)
1192     out[l1t::RegionalMuonCand::kWheelNum] = 0;
1193   else if (track.wheel() == 1)
1194     out[l1t::RegionalMuonCand::kWheelNum] = 1;
1195   else if (track.wheel() == 2)
1196     out[l1t::RegionalMuonCand::kWheelNum] = 2;
1197   else
1198     out[l1t::RegionalMuonCand::kWheelNum] = 0;
1199   out[l1t::RegionalMuonCand::kStat1] = 15;
1200   out[l1t::RegionalMuonCand::kStat2] = 15;
1201   out[l1t::RegionalMuonCand::kStat3] = 15;
1202   out[l1t::RegionalMuonCand::kStat4] = 3;
1203   out[l1t::RegionalMuonCand::kSegSelStat1] = 0;
1204   out[l1t::RegionalMuonCand::kSegSelStat2] = 0;
1205   out[l1t::RegionalMuonCand::kSegSelStat3] = 0;
1206   out[l1t::RegionalMuonCand::kSegSelStat4] = 0;
1207   //out[l1t::RegionalMuonCand::kNumBmtfSubAddr]=0; // This is commented out for better data/MC agreement
1208 
1209   for (const auto& stub : track.stubs()) {
1210     bool ownwheel = stub->whNum() == track.wheel();
1211     int sector = 0;
1212     if ((stub->scNum() == track.sector() + 1) || (stub->scNum() == 0 && track.sector() == 11))
1213       sector = +1;
1214     if ((stub->scNum() == track.sector() - 1) || (stub->scNum() == 11 && track.sector() == 0))
1215       sector = -1;
1216     int addr = encode(ownwheel, sector, stub->tag());
1217 
1218     if (stub->stNum() == 4) {
1219       if (stub->tag())
1220         addr = 1;
1221       else
1222         addr = 2;
1223       out[l1t::RegionalMuonCand::kStat4] = addr;
1224     }
1225     if (stub->stNum() == 3) {
1226       out[l1t::RegionalMuonCand::kStat3] = addr;
1227     }
1228     if (stub->stNum() == 2) {
1229       out[l1t::RegionalMuonCand::kStat2] = addr;
1230     }
1231     if (stub->stNum() == 1) {
1232       out[l1t::RegionalMuonCand::kStat1] = addr;
1233     }
1234   }
1235 
1236   word = 0;
1237   word = word | out[l1t::RegionalMuonCand::kStat4] << 12;
1238   word = word | out[l1t::RegionalMuonCand::kStat3] << 8;
1239   word = word | out[l1t::RegionalMuonCand::kStat2] << 4;
1240   word = word | out[l1t::RegionalMuonCand::kStat1];
1241 
1242   return out;
1243 }
1244 
1245 uint L1TMuonBarrelKalmanAlgo::twosCompToBits(int q) {
1246   if (q >= 0)
1247     return q;
1248   else
1249     return (~q) + 1;
1250 }
1251 
1252 int L1TMuonBarrelKalmanAlgo::fp_product(float a, int b, uint bits) {
1253   //  return long(a*(1<<bits)*b)>>bits;
1254   return (long((a * (1 << bits)) * b)) >> bits;
1255 }
1256 
1257 int L1TMuonBarrelKalmanAlgo::ptLUT(int K) {
1258   int charge = (K >= 0) ? +1 : -1;
1259   float lsb = 1.25 / float(1 << 13);
1260   float FK = fabs(K);
1261 
1262   if (FK > 2047)
1263     FK = 2047.;
1264   if (FK < 26)
1265     FK = 26.;
1266 
1267   FK = FK * lsb;
1268 
1269   //step 1 -material and B-field
1270   FK = .8569 * FK / (1.0 + 0.1144 * FK);
1271   //step 2 - misalignment
1272   FK = FK - charge * 1.23e-03;
1273   //Get to BMTF scale
1274   FK = FK / 1.17;
1275 
1276   int pt = 0;
1277   if (FK != 0)
1278     pt = int(2.0 / FK);
1279 
1280   if (pt > 511)
1281     pt = 511;
1282 
1283   if (pt < 8)
1284     pt = 8;
1285 
1286   return pt;
1287 }
1288 
1289 L1MuKBMTrackCollection L1TMuonBarrelKalmanAlgo::clean(const L1MuKBMTrackCollection& tracks, uint seed) {
1290   L1MuKBMTrackCollection out;
1291 
1292   std::map<uint, int> infoRank;
1293   std::map<uint, L1MuKBMTrack> infoTrack;
1294   for (uint i = 3; i <= 15; ++i) {
1295     if (i == 4 || i == 8)
1296       continue;
1297     infoRank[i] = -1;
1298   }
1299 
1300   for (const auto& track : tracks) {
1301     infoRank[track.hitPattern()] = rank(track);
1302     infoTrack[track.hitPattern()] = track;
1303   }
1304 
1305   int selected = 15;
1306   if (seed == 4)  //station 4 seeded
1307   {
1308     int sel6 = infoRank[10] >= infoRank[12] ? 10 : 12;
1309     int sel5 = infoRank[14] >= infoRank[9] ? 14 : 9;
1310     int sel4 = infoRank[11] >= infoRank[13] ? 11 : 13;
1311     int sel3 = infoRank[sel6] >= infoRank[sel5] ? sel6 : sel5;
1312     int sel2 = infoRank[sel4] >= infoRank[sel3] ? sel4 : sel3;
1313     selected = infoRank[15] >= infoRank[sel2] ? 15 : sel2;
1314   }
1315   if (seed == 3)  //station 3 seeded
1316   {
1317     int sel2 = infoRank[5] >= infoRank[6] ? 5 : 6;
1318     selected = infoRank[7] >= infoRank[sel2] ? 7 : sel2;
1319   }
1320   if (seed == 2)  //station 2 seeded
1321     selected = 3;
1322 
1323   auto search = infoTrack.find(selected);
1324   if (search != infoTrack.end())
1325     out.push_back(search->second);
1326 
1327   return out;
1328 }
1329 
1330 uint L1TMuonBarrelKalmanAlgo::etaStubRank(const L1MuKBMTCombinedStubRef& stub) {
1331   if (stub->qeta1() != 0 && stub->qeta2() != 0) {
1332     return 0;
1333   }
1334   if (stub->qeta1() == 0) {
1335     return 0;
1336   }
1337   //  return (stub->qeta1()*4+stub->stNum());
1338   return (stub->qeta1());
1339 }
1340 
1341 void L1TMuonBarrelKalmanAlgo::calculateEta(L1MuKBMTrack& track) {
1342   uint pattern = track.hitPattern();
1343   int wheel = track.stubs()[0]->whNum();
1344   uint awheel = fabs(wheel);
1345   int sign = 1;
1346   if (wheel < 0)
1347     sign = -1;
1348   uint nstubs = track.stubs().size();
1349   uint mask = 0;
1350   for (unsigned int i = 0; i < track.stubs().size(); ++i) {
1351     if (fabs(track.stubs()[i]->whNum()) != awheel)
1352       mask = mask | (1 << i);
1353   }
1354   mask = (awheel << nstubs) | mask;
1355   track.setCoarseEta(sign * lutService_->coarseEta(pattern, mask));
1356   int sumweights = 0;
1357   int sums = 0;
1358 
1359   for (const auto& stub : track.stubs()) {
1360     uint rank = etaStubRank(stub);
1361     if (rank == 0)
1362       continue;
1363     //    printf("Stub station=%d rank=%d values=%d %d\n",stub->stNum(),rank,stub->eta1(),stub->eta2());
1364     sumweights += rank;
1365     sums += rank * stub->eta1();
1366   }
1367   //0.5  0.332031 0.25 0.199219 0.164063
1368   float factor;
1369   if (sumweights == 1)
1370     factor = 1.0;
1371   else if (sumweights == 2)
1372     factor = 0.5;
1373   else if (sumweights == 3)
1374     factor = 0.332031;
1375   else if (sumweights == 4)
1376     factor = 0.25;
1377   else if (sumweights == 5)
1378     factor = 0.199219;
1379   else if (sumweights == 6)
1380     factor = 0.164063;
1381   else
1382     factor = 0.0;
1383 
1384   int eta = 0;
1385   if (sums > 0)
1386     eta = fp_product(factor, sums, 10);
1387   else
1388     eta = -fp_product(factor, fabs(sums), 10);
1389 
1390   //int eta=int(factor*sums);
1391   //    printf("Eta debug %f *%d=%d\n",factor,sums,eta);
1392 
1393   if (sumweights > 0)
1394     track.setFineEta(eta);
1395 }
1396 
1397 int L1TMuonBarrelKalmanAlgo::phiAt2(const L1MuKBMTrack& track) {
1398   //If there is stub at station 2 use this else propagate from 1
1399   for (const auto& stub : track.stubs())
1400     if (stub->stNum() == 2)
1401       return correctedPhi(stub, track.sector());
1402 
1403   ap_fixed<BITSPHI, BITSPHI> phi = track.phiAtMuon();
1404   ap_fixed<BITSPHIB, BITSPHIB> phiB = track.phiBAtMuon();
1405   ap_fixed<BITSPARAM, 1> phiAt2 = phiAt2_;
1406   int phiNew = ap_fixed<BITSPHI + 1, BITSPHI + 1, AP_TRN_ZERO, AP_SAT>(phi + phiAt2 * phiB);
1407 
1408   if (verbose_)
1409     printf("Phi at second station=%d\n", phiNew);
1410   return phiNew;
1411 }
[ 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!