Back to home page

Project CMSSW displayed by LXR

 
 

    


File indexing completed on 2024-04-06 12:20:52

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