Back to home page

Project CMSSW displayed by LXR

 
 

    


File indexing completed on 2021-07-07 22:33:31

0001 #include "L1Trigger/TrackFindingTracklet/interface/FitTrack.h"
0002 #include "L1Trigger/TrackFindingTracklet/interface/Globals.h"
0003 #include "L1Trigger/TrackFindingTracklet/interface/TrackDerTable.h"
0004 #include "L1Trigger/TrackFindingTracklet/interface/HybridFit.h"
0005 #include "L1Trigger/TrackFindingTracklet/interface/Tracklet.h"
0006 #include "L1Trigger/TrackFindingTracklet/interface/Stub.h"
0007 
0008 #include "FWCore/MessageLogger/interface/MessageLogger.h"
0009 #include "FWCore/Utilities/interface/Exception.h"
0010 
0011 using namespace std;
0012 using namespace trklet;
0013 
0014 FitTrack::FitTrack(string name, Settings const& settings, Globals* global)
0015     : ProcessBase(name, settings, global), trackfit_(nullptr) {}
0016 
0017 void FitTrack::addOutput(MemoryBase* memory, string output) {
0018   if (settings_.writetrace()) {
0019     edm::LogVerbatim("Tracklet") << "In " << name_ << " adding output to " << memory->getName() << " to output "
0020                                  << output;
0021   }
0022   if (output == "trackout") {
0023     TrackFitMemory* tmp = dynamic_cast<TrackFitMemory*>(memory);
0024     assert(tmp != nullptr);
0025     trackfit_ = tmp;
0026     return;
0027   }
0028 
0029   throw cms::Exception("BadConfig") << __FILE__ << " " << __LINE__ << " addOutput, output = " << output << " not known";
0030 }
0031 
0032 void FitTrack::addInput(MemoryBase* memory, string input) {
0033   if (settings_.writetrace()) {
0034     edm::LogVerbatim("Tracklet") << "In " << name_ << " adding input from " << memory->getName() << " to input "
0035                                  << input;
0036   }
0037   if (input.substr(0, 4) == "tpar") {
0038     auto* tmp = dynamic_cast<TrackletParametersMemory*>(memory);
0039     assert(tmp != nullptr);
0040     seedtracklet_.push_back(tmp);
0041     return;
0042   }
0043   if (input.substr(0, 10) == "fullmatch1") {
0044     auto* tmp = dynamic_cast<FullMatchMemory*>(memory);
0045     assert(tmp != nullptr);
0046     fullmatch1_.push_back(tmp);
0047     return;
0048   }
0049   if (input.substr(0, 10) == "fullmatch2") {
0050     auto* tmp = dynamic_cast<FullMatchMemory*>(memory);
0051     assert(tmp != nullptr);
0052     fullmatch2_.push_back(tmp);
0053     return;
0054   }
0055   if (input.substr(0, 10) == "fullmatch3") {
0056     auto* tmp = dynamic_cast<FullMatchMemory*>(memory);
0057     assert(tmp != nullptr);
0058     fullmatch3_.push_back(tmp);
0059     return;
0060   }
0061   if (input.substr(0, 10) == "fullmatch4") {
0062     auto* tmp = dynamic_cast<FullMatchMemory*>(memory);
0063     assert(tmp != nullptr);
0064     fullmatch4_.push_back(tmp);
0065     return;
0066   }
0067 
0068   throw cms::Exception("BadConfig") << __FILE__ << " " << __LINE__ << " input = " << input << " not found";
0069 }
0070 
0071 #ifdef USEHYBRID
0072 void FitTrack::trackFitKF(Tracklet* tracklet,
0073                           std::vector<const Stub*>& trackstublist,
0074                           std::vector<std::pair<int, int>>& stubidslist) {
0075   if (settings_.doKF()) {
0076     // From full match lists, collect all the stubs associated with the tracklet seed
0077 
0078     // Get seed stubs first
0079     trackstublist.emplace_back(tracklet->innerFPGAStub());
0080     if (tracklet->getISeed() >= (int)N_TRKLSEED + 1)
0081       trackstublist.emplace_back(tracklet->middleFPGAStub());
0082     trackstublist.emplace_back(tracklet->outerFPGAStub());
0083 
0084     // Now get ALL matches (can have multiple per layer)
0085     for (const auto& i : fullmatch1_) {
0086       for (unsigned int j = 0; j < i->nMatches(); j++) {
0087         if (i->getTracklet(j)->TCID() == tracklet->TCID()) {
0088           trackstublist.push_back(i->getMatch(j).second);
0089         }
0090       }
0091     }
0092 
0093     for (const auto& i : fullmatch2_) {
0094       for (unsigned int j = 0; j < i->nMatches(); j++) {
0095         if (i->getTracklet(j)->TCID() == tracklet->TCID()) {
0096           trackstublist.push_back(i->getMatch(j).second);
0097         }
0098       }
0099     }
0100 
0101     for (const auto& i : fullmatch3_) {
0102       for (unsigned int j = 0; j < i->nMatches(); j++) {
0103         if (i->getTracklet(j)->TCID() == tracklet->TCID()) {
0104           trackstublist.push_back(i->getMatch(j).second);
0105         }
0106       }
0107     }
0108 
0109     for (const auto& i : fullmatch4_) {
0110       for (unsigned int j = 0; j < i->nMatches(); j++) {
0111         if (i->getTracklet(j)->TCID() == tracklet->TCID()) {
0112           trackstublist.push_back(i->getMatch(j).second);
0113         }
0114       }
0115     }
0116 
0117     // For merge removal, loop through the resulting list of stubs to calculate their stubids
0118     if (settings_.removalType() == "merge") {
0119       for (const auto& it : trackstublist) {
0120         int layer = it->layer().value() + 1;  // Assume layer (1-6) stub first
0121         if (it->layer().value() < 0) {        // if disk stub, though...
0122           layer = it->disk().value() + 10 * it->disk().value() / abs(it->disk().value());  //disk = +/- 11-15
0123         }
0124         stubidslist.push_back(std::make_pair(layer, it->phiregionaddress()));
0125       }
0126 
0127       // And that's all we need! The rest is just for fitting (in PurgeDuplicate)
0128       return;
0129     }
0130 
0131     HybridFit hybridFitter(iSector_, settings_, globals_);
0132     hybridFitter.Fit(tracklet, trackstublist);
0133     return;
0134   }
0135 }
0136 #endif
0137 
0138 void FitTrack::trackFitChisq(Tracklet* tracklet, std::vector<const Stub*>&, std::vector<std::pair<int, int>>&) {
0139   if (globals_->trackDerTable() == nullptr) {
0140     TrackDerTable* derTablePtr = new TrackDerTable(settings_);
0141 
0142     derTablePtr->readPatternFile(settings_.fitPatternFile());
0143     derTablePtr->fillTable();
0144     if (settings_.debugTracklet()) {
0145       edm::LogVerbatim("Tracklet") << "Number of entries in derivative table: " << derTablePtr->getEntries();
0146     }
0147     assert(derTablePtr->getEntries() != 0);
0148 
0149     globals_->trackDerTable() = derTablePtr;
0150   }
0151 
0152   const TrackDerTable& derTable = *globals_->trackDerTable();
0153 
0154   //First step is to build list of layers and disks.
0155   int layers[N_LAYER];
0156   double r[N_LAYER];
0157   unsigned int nlayers = 0;  // layers with found stub-projections
0158   int disks[N_DISK];
0159   double z[N_DISK];
0160   unsigned int ndisks = 0;  // disks with found stub-projections
0161 
0162   // residuals for each stub
0163   double phiresid[N_FITSTUB];
0164   double zresid[N_FITSTUB];
0165   double phiresidexact[N_FITSTUB];
0166   double zresidexact[N_FITSTUB];
0167   int iphiresid[N_FITSTUB];
0168   int izresid[N_FITSTUB];
0169   double alpha[N_FITSTUB];
0170 
0171   for (unsigned int i = 0; i < N_FITSTUB; i++) {
0172     iphiresid[i] = 0;
0173     izresid[i] = 0;
0174     alpha[i] = 0.0;
0175 
0176     phiresid[i] = 0.0;
0177     zresid[i] = 0.0;
0178     phiresidexact[i] = 0.0;
0179     zresidexact[i] = 0.0;
0180   }
0181 
0182   std::bitset<N_LAYER> lmatches;     //layer matches
0183   std::bitset<N_DISK * 2> dmatches;  //disk matches (2 per disk to separate 2S from PS)
0184 
0185   int mult = 1;
0186 
0187   unsigned int layermask = 0;
0188   unsigned int diskmask = 0;
0189   unsigned int alphaindex = 0;
0190   unsigned int power = 1;
0191 
0192   double t = tracklet->t();
0193   double rinv = tracklet->rinv();
0194 
0195   if (tracklet->isBarrel()) {
0196     for (unsigned int l = 1; l <= N_LAYER; l++) {
0197       if (l == (unsigned int)tracklet->layer() || l == (unsigned int)tracklet->layer() + 1) {
0198         lmatches.set(N_LAYER - l);
0199         layermask |= (1 << (N_LAYER - l));
0200         layers[nlayers++] = l;
0201         continue;
0202       }
0203       if (tracklet->match(l - 1)) {
0204         const Residual& resid = tracklet->resid(l - 1);
0205         lmatches.set(N_LAYER - l);
0206         layermask |= (1 << (N_LAYER - l));
0207         phiresid[nlayers] = resid.phiresidapprox();
0208         zresid[nlayers] = resid.rzresidapprox();
0209         phiresidexact[nlayers] = resid.phiresid();
0210         zresidexact[nlayers] = resid.rzresid();
0211         iphiresid[nlayers] = resid.fpgaphiresid().value();
0212         izresid[nlayers] = resid.fpgarzresid().value();
0213 
0214         layers[nlayers++] = l;
0215       }
0216     }
0217 
0218     for (unsigned int d = 1; d <= N_DISK; d++) {
0219       if (layermask & (1 << (d - 1)))
0220         continue;
0221 
0222       if (mult == 1 << (3 * settings_.alphaBitsTable()))
0223         continue;
0224 
0225       if (ndisks + nlayers >= N_FITSTUB)
0226         continue;
0227       if (tracklet->match(N_LAYER + d - 1)) {
0228         const Residual& resid = tracklet->resid(N_LAYER + d - 1);
0229         double pitch = settings_.stripPitch(resid.stubptr()->l1tstub()->isPSmodule());
0230         if (std::abs(resid.stubptr()->l1tstub()->alpha(pitch)) < 1e-20) {
0231           dmatches.set(2 * d - 1);
0232           diskmask |= (1 << (2 * (N_DISK - d) + 1));
0233         } else {
0234           int ialpha = resid.stubptr()->alpha().value();
0235           int nalpha = resid.stubptr()->alpha().nbits();
0236           nalpha = nalpha - settings_.alphaBitsTable();
0237           ialpha = (1 << (settings_.alphaBitsTable() - 1)) + (ialpha >> nalpha);
0238 
0239           alphaindex += ialpha * power;
0240           power = power << settings_.alphaBitsTable();
0241           dmatches.set(2 * (N_DISK - d));
0242           diskmask |= (1 << (2 * (N_DISK - d)));
0243           mult = mult << settings_.alphaBitsTable();
0244         }
0245         alpha[ndisks] = resid.stubptr()->l1tstub()->alpha(pitch);
0246         phiresid[nlayers + ndisks] = resid.phiresidapprox();
0247         zresid[nlayers + ndisks] = resid.rzresidapprox();
0248         phiresidexact[nlayers + ndisks] = resid.phiresid();
0249         zresidexact[nlayers + ndisks] = resid.rzresid();
0250         iphiresid[nlayers + ndisks] = resid.fpgaphiresid().value();
0251         izresid[nlayers + ndisks] = resid.fpgarzresid().value();
0252 
0253         disks[ndisks++] = d;
0254       }
0255     }
0256 
0257     if (settings_.writeMonitorData("HitPattern")) {
0258       if (mult <= 1 << (3 * settings_.alphaBitsTable())) {
0259         globals_->ofstream("hitpattern.txt")
0260             << lmatches.to_string() << " " << dmatches.to_string() << " " << mult << endl;
0261       }
0262     }
0263   }
0264 
0265   if (tracklet->isDisk()) {
0266     for (unsigned int l = 1; l <= 2; l++) {
0267       if (tracklet->match(l - 1)) {
0268         lmatches.set(N_LAYER - l);
0269 
0270         layermask |= (1 << (N_LAYER - l));
0271         const Residual& resid = tracklet->resid(l - 1);
0272         phiresid[nlayers] = resid.phiresidapprox();
0273         zresid[nlayers] = resid.rzresidapprox();
0274         phiresidexact[nlayers] = resid.phiresid();
0275         zresidexact[nlayers] = resid.rzresid();
0276         iphiresid[nlayers] = resid.fpgaphiresid().value();
0277         izresid[nlayers] = resid.fpgarzresid().value();
0278 
0279         layers[nlayers++] = l;
0280       }
0281     }
0282 
0283     for (int d1 = 1; d1 <= N_DISK; d1++) {
0284       int d = d1;
0285 
0286       // skip F/B5 if there's already a L2 match
0287       if (d == 5 and layermask & (1 << 4))
0288         continue;
0289 
0290       if (tracklet->fpgat().value() < 0.0)
0291         d = -d1;
0292       if (d1 == abs(tracklet->disk()) || d1 == abs(tracklet->disk()) + 1) {
0293         dmatches.set(2 * d1 - 1);
0294         diskmask |= (1 << (2 * (N_DISK - d1) + 1));
0295         alpha[ndisks] = 0.0;
0296         disks[ndisks++] = d;
0297         continue;
0298       }
0299 
0300       if (ndisks + nlayers >= N_FITSTUB)
0301         continue;
0302       if (tracklet->match(N_LAYER + abs(d) - 1)) {
0303         const Residual& resid = tracklet->resid(N_LAYER + abs(d) - 1);
0304         double pitch = settings_.stripPitch(resid.stubptr()->l1tstub()->isPSmodule());
0305         if (std::abs(resid.stubptr()->l1tstub()->alpha(pitch)) < 1e-20) {
0306           dmatches.set(2 * d1 - 1);
0307           diskmask |= (1 << (2 * (N_DISK - d1) + 1));
0308         } else {
0309           int ialpha = resid.stubptr()->alpha().value();
0310           int nalpha = resid.stubptr()->alpha().nbits();
0311           nalpha = nalpha - settings_.alphaBitsTable();
0312           ialpha = (1 << (settings_.alphaBitsTable() - 1)) + (ialpha >> nalpha);
0313 
0314           alphaindex += ialpha * power;
0315           power = power << settings_.alphaBitsTable();
0316           dmatches.set(2 * (N_DISK - d1));
0317           diskmask |= (1 << (2 * (N_DISK - d1)));
0318           mult = mult << settings_.alphaBitsTable();
0319         }
0320 
0321         alpha[ndisks] = resid.stubptr()->l1tstub()->alpha(pitch);
0322         assert(std::abs(resid.phiresidapprox()) < 0.2);
0323         phiresid[nlayers + ndisks] = resid.phiresidapprox();
0324         zresid[nlayers + ndisks] = resid.rzresidapprox();
0325         assert(std::abs(resid.phiresid()) < 0.2);
0326         phiresidexact[nlayers + ndisks] = resid.phiresid();
0327         zresidexact[nlayers + ndisks] = resid.rzresid();
0328         iphiresid[nlayers + ndisks] = resid.fpgaphiresid().value();
0329         izresid[nlayers + ndisks] = resid.fpgarzresid().value();
0330 
0331         disks[ndisks++] = d;
0332       }
0333     }
0334   }
0335 
0336   if (tracklet->isOverlap()) {
0337     for (unsigned int l = 1; l <= 2; l++) {
0338       if (l == (unsigned int)tracklet->layer()) {
0339         lmatches.set(N_LAYER - l);
0340         layermask |= (1 << (N_LAYER - l));
0341         layers[nlayers++] = l;
0342         continue;
0343       }
0344       if (tracklet->match(l - 1)) {
0345         lmatches.set(N_LAYER - l);
0346         layermask |= (1 << (N_LAYER - l));
0347         const Residual& resid = tracklet->resid(l - 1);
0348         assert(std::abs(resid.phiresidapprox()) < 0.2);
0349         phiresid[nlayers] = resid.phiresidapprox();
0350         zresid[nlayers] = resid.rzresidapprox();
0351         assert(std::abs(resid.phiresid()) < 0.2);
0352         phiresidexact[nlayers] = resid.phiresid();
0353         zresidexact[nlayers] = resid.rzresid();
0354         iphiresid[nlayers] = resid.fpgaphiresid().value();
0355         izresid[nlayers] = resid.fpgarzresid().value();
0356 
0357         layers[nlayers++] = l;
0358       }
0359     }
0360 
0361     for (unsigned int d1 = 1; d1 <= N_DISK; d1++) {
0362       if (mult == 1 << (3 * settings_.alphaBitsTable()))
0363         continue;
0364       int d = d1;
0365       if (tracklet->fpgat().value() < 0.0)
0366         d = -d1;
0367       if (d == tracklet->disk()) {  //All seeds in PS modules
0368         disks[ndisks] = tracklet->disk();
0369         dmatches.set(2 * d1 - 1);
0370         diskmask |= (1 << (2 * (N_DISK - d1) + 1));
0371         ndisks++;
0372         continue;
0373       }
0374 
0375       if (ndisks + nlayers >= N_FITSTUB)
0376         continue;
0377       if (tracklet->match(N_LAYER + abs(d) - 1)) {
0378         const Residual& resid = tracklet->resid(N_LAYER + abs(d) - 1);
0379         double pitch = settings_.stripPitch(resid.stubptr()->l1tstub()->isPSmodule());
0380         if (std::abs(resid.stubptr()->l1tstub()->alpha(pitch)) < 1e-20) {
0381           dmatches.set(2 * (N_DISK - d1));
0382           diskmask |= (1 << (2 * (N_DISK - d1) + 1));
0383           FPGAWord tmp;
0384           tmp.set(diskmask, 10);
0385         } else {
0386           int ialpha = resid.stubptr()->alpha().value();
0387           int nalpha = resid.stubptr()->alpha().nbits();
0388           nalpha = nalpha - settings_.alphaBitsTable();
0389           ialpha = (1 << (settings_.alphaBitsTable() - 1)) + (ialpha >> nalpha);
0390 
0391           alphaindex += ialpha * power;
0392           power = power << settings_.alphaBitsTable();
0393           dmatches.set(2 * (N_DISK - d1));
0394           diskmask |= (1 << (2 * (N_DISK - d1)));
0395           FPGAWord tmp;
0396           tmp.set(diskmask, 10);
0397           mult = mult << settings_.alphaBitsTable();
0398         }
0399 
0400         alpha[ndisks] = resid.stubptr()->l1tstub()->alpha(pitch);
0401         assert(std::abs(resid.phiresidapprox()) < 0.2);
0402         phiresid[nlayers + ndisks] = resid.phiresidapprox();
0403         zresid[nlayers + ndisks] = resid.rzresidapprox();
0404         assert(std::abs(resid.phiresid()) < 0.2);
0405         phiresidexact[nlayers + ndisks] = resid.phiresid();
0406         zresidexact[nlayers + ndisks] = resid.rzresid();
0407         iphiresid[nlayers + ndisks] = resid.fpgaphiresid().value();
0408         izresid[nlayers + ndisks] = resid.fpgarzresid().value();
0409 
0410         disks[ndisks++] = d;
0411       }
0412     }
0413   }
0414 
0415   int rinvindex =
0416       (1 << (settings_.nrinvBitsTable() - 1)) * rinv / settings_.rinvmax() + (1 << (settings_.nrinvBitsTable() - 1));
0417   if (rinvindex < 0)
0418     rinvindex = 0;
0419   if (rinvindex >= (1 << settings_.nrinvBitsTable()))
0420     rinvindex = (1 << settings_.nrinvBitsTable()) - 1;
0421 
0422   const TrackDer* derivatives = derTable.getDerivatives(layermask, diskmask, alphaindex, rinvindex);
0423 
0424   if (derivatives == nullptr) {
0425     if (settings_.warnNoDer()) {
0426       FPGAWord tmpl, tmpd;
0427       tmpl.set(layermask, 6);
0428       tmpd.set(diskmask, 10);
0429       edm::LogVerbatim("Tracklet") << "No derivative for layermask, diskmask : " << layermask << " " << tmpl.str()
0430                                    << " " << diskmask << " " << tmpd.str() << " eta = " << asinh(t);
0431     }
0432     return;
0433   }
0434 
0435   double ttabi = TrackDerTable::tpar(settings_, diskmask, layermask);
0436   if (t < 0.0)
0437     ttabi = -ttabi;
0438   double ttab = ttabi;
0439 
0440   if (settings_.debugTracklet()) {
0441     edm::LogVerbatim("Tracklet") << "Doing trackfit in  " << getName();
0442   }
0443 
0444   int sign = 1;
0445   if (t < 0.0)
0446     sign = -1;
0447 
0448   double rstub[6];
0449 
0450   double realrstub[3];
0451   realrstub[0] = -1.0;
0452   realrstub[1] = -1.0;
0453   realrstub[2] = -1.0;
0454 
0455   for (unsigned i = 0; i < nlayers; i++) {
0456     r[i] = settings_.rmean(layers[i] - 1);
0457     if (layers[i] == tracklet->layer()) {
0458       if (tracklet->isOverlap()) {
0459         realrstub[i] = tracklet->outerFPGAStub()->l1tstub()->r();
0460       } else {
0461         realrstub[i] = tracklet->innerFPGAStub()->l1tstub()->r();
0462       }
0463     }
0464     if (layers[i] == tracklet->layer() + 1) {
0465       realrstub[i] = tracklet->outerFPGAStub()->l1tstub()->r();
0466     }
0467     if (tracklet->match(layers[i] - 1) && layers[i] < 4) {
0468       const Stub* stubptr = tracklet->resid(layers[i] - 1).stubptr();
0469       realrstub[i] = stubptr->l1tstub()->r();
0470       assert(std::abs(realrstub[i] - r[i]) < 5.0);
0471     }
0472     rstub[i] = r[i];
0473   }
0474   for (unsigned i = 0; i < ndisks; i++) {
0475     z[i] = sign * settings_.zmean(abs(disks[i]) - 1);
0476     rstub[i + nlayers] = z[i] / ttabi;
0477   }
0478 
0479   double D[N_FITPARAM][N_FITSTUB * 2];
0480   double MinvDt[N_FITPARAM][N_FITSTUB * 2];
0481   int iD[N_FITPARAM][N_FITSTUB * 2];
0482   int iMinvDt[N_FITPARAM][N_FITSTUB * 2];
0483   double sigma[N_FITSTUB * 2];
0484   double kfactor[N_FITSTUB * 2];
0485 
0486   unsigned int n = nlayers + ndisks;
0487 
0488   if (settings_.exactderivatives()) {
0489     TrackDerTable::calculateDerivatives(
0490         settings_, nlayers, r, ndisks, z, alpha, t, rinv, D, iD, MinvDt, iMinvDt, sigma, kfactor);
0491     ttabi = t;
0492     ttab = t;
0493   } else {
0494     if (settings_.exactderivativesforfloating()) {
0495       TrackDerTable::calculateDerivatives(
0496           settings_, nlayers, r, ndisks, z, alpha, t, rinv, D, iD, MinvDt, iMinvDt, sigma, kfactor);
0497 
0498       double MinvDtDummy[N_FITPARAM][N_FITSTUB * 2];
0499       derivatives->fill(tracklet->fpgat().value(), MinvDtDummy, iMinvDt);
0500       ttab = t;
0501     } else {
0502       derivatives->fill(tracklet->fpgat().value(), MinvDt, iMinvDt);
0503     }
0504   }
0505 
0506   if (!settings_.exactderivatives()) {
0507     for (unsigned int i = 0; i < nlayers; i++) {
0508       if (r[i] > settings_.rPS2S())
0509         continue;
0510       for (unsigned int ii = 0; ii < nlayers; ii++) {
0511         if (r[ii] > settings_.rPS2S())
0512           continue;
0513 
0514         double tder = derivatives->tdzcorr(i, ii);
0515         double zder = derivatives->z0dzcorr(i, ii);
0516 
0517         double dr = realrstub[i] - r[i];
0518 
0519         MinvDt[2][2 * ii + 1] += dr * tder;
0520         MinvDt[3][2 * ii + 1] += dr * zder;
0521 
0522         int itder = derivatives->itdzcorr(i, ii);
0523         int izder = derivatives->iz0dzcorr(i, ii);
0524 
0525         int idr = dr / settings_.kr();
0526 
0527         iMinvDt[2][2 * ii + 1] += ((idr * itder) >> settings_.rcorrbits());
0528         iMinvDt[3][2 * ii + 1] += ((idr * izder) >> settings_.rcorrbits());
0529       }
0530     }
0531   }
0532 
0533   double rinvseed = tracklet->rinvapprox();
0534   double phi0seed = tracklet->phi0approx();
0535   double tseed = tracklet->tapprox();
0536   double z0seed = tracklet->z0approx();
0537 
0538   double rinvseedexact = tracklet->rinv();
0539   double phi0seedexact = tracklet->phi0();
0540   double tseedexact = tracklet->t();
0541   double z0seedexact = tracklet->z0();
0542 
0543   double chisqseed = 0.0;
0544   double chisqseedexact = 0.0;
0545 
0546   double delta[2 * N_FITSTUB];
0547   double deltaexact[2 * N_FITSTUB];
0548   int idelta[2 * N_FITSTUB];
0549 
0550   for (unsigned int i = 0; i < 2 * N_FITSTUB; i++) {
0551     delta[i] = 0.0;
0552     deltaexact[i] = 0.0;
0553     idelta[i] = 0;
0554   }
0555 
0556   int j = 0;
0557 
0558   for (unsigned int i = 0; i < n; i++) {
0559     if (i >= nlayers) {
0560       iphiresid[i] *= (t / ttabi);
0561       phiresid[i] *= (t / ttab);
0562       phiresidexact[i] *= (t / ttab);
0563     }
0564 
0565     idelta[j] = iphiresid[i];
0566     delta[j] = phiresid[i];
0567     if (std::abs(phiresid[i]) > 0.2) {
0568       edm::LogWarning("Tracklet") << getName() << " WARNING too large phiresid: " << phiresid[i] << " "
0569                                   << phiresidexact[i];
0570     }
0571     assert(std::abs(phiresid[i]) < 1.0);
0572     assert(std::abs(phiresidexact[i]) < 1.0);
0573     deltaexact[j++] = phiresidexact[i];
0574 
0575     idelta[j] = izresid[i];
0576     delta[j] = zresid[i];
0577     deltaexact[j++] = zresidexact[i];
0578 
0579     chisqseed += (delta[j - 2] * delta[j - 2] + delta[j - 1] * delta[j - 1]);
0580     chisqseedexact += (deltaexact[j - 2] * deltaexact[j - 2] + deltaexact[j - 1] * deltaexact[j - 1]);
0581   }
0582   assert(j <= 12);
0583 
0584   double drinv = 0.0;
0585   double dphi0 = 0.0;
0586   double dt = 0.0;
0587   double dz0 = 0.0;
0588 
0589   double drinvexact = 0.0;
0590   double dphi0exact = 0.0;
0591   double dtexact = 0.0;
0592   double dz0exact = 0.0;
0593 
0594   int idrinv = 0;
0595   int idphi0 = 0;
0596   int idt = 0;
0597   int idz0 = 0;
0598 
0599   double drinv_cov = 0.0;
0600   double dphi0_cov = 0.0;
0601   double dt_cov = 0.0;
0602   double dz0_cov = 0.0;
0603 
0604   double drinv_covexact = 0.0;
0605   double dphi0_covexact = 0.0;
0606   double dt_covexact = 0.0;
0607   double dz0_covexact = 0.0;
0608 
0609   for (unsigned int j = 0; j < 2 * n; j++) {
0610     drinv -= MinvDt[0][j] * delta[j];
0611     dphi0 -= MinvDt[1][j] * delta[j];
0612     dt -= MinvDt[2][j] * delta[j];
0613     dz0 -= MinvDt[3][j] * delta[j];
0614 
0615     drinv_cov += D[0][j] * delta[j];
0616     dphi0_cov += D[1][j] * delta[j];
0617     dt_cov += D[2][j] * delta[j];
0618     dz0_cov += D[3][j] * delta[j];
0619 
0620     drinvexact -= MinvDt[0][j] * deltaexact[j];
0621     dphi0exact -= MinvDt[1][j] * deltaexact[j];
0622     dtexact -= MinvDt[2][j] * deltaexact[j];
0623     dz0exact -= MinvDt[3][j] * deltaexact[j];
0624 
0625     drinv_covexact += D[0][j] * deltaexact[j];
0626     dphi0_covexact += D[1][j] * deltaexact[j];
0627     dt_covexact += D[2][j] * deltaexact[j];
0628     dz0_covexact += D[3][j] * deltaexact[j];
0629 
0630     idrinv += ((iMinvDt[0][j] * idelta[j]));
0631     idphi0 += ((iMinvDt[1][j] * idelta[j]));
0632     idt += ((iMinvDt[2][j] * idelta[j]));
0633     idz0 += ((iMinvDt[3][j] * idelta[j]));
0634 
0635     if (false && j % 2 == 0) {
0636       edm::LogVerbatim("Tracklet") << "DEBUG CHI2FIT " << j << " " << rinvseed << " + " << MinvDt[0][j] * delta[j]
0637                                    << " " << MinvDt[0][j] << " " << delta[j] * rstub[j / 2] * 10000 << " \n"
0638                                    << j << " " << tracklet->fpgarinv().value() * settings_.krinvpars() << " + "
0639                                    << ((iMinvDt[0][j] * idelta[j])) * settings_.krinvpars() / 1024.0 << " "
0640                                    << iMinvDt[0][j] * settings_.krinvpars() / settings_.kphi() / 1024.0 << " "
0641                                    << idelta[j] * settings_.kphi() * rstub[j / 2] * 10000 << " " << idelta[j];
0642     }
0643   }
0644 
0645   double deltaChisqexact =
0646       drinvexact * drinv_covexact + dphi0exact * dphi0_covexact + dtexact * dt_covexact + dz0exact * dz0_covexact;
0647 
0648   int irinvseed = tracklet->fpgarinv().value();
0649   int iphi0seed = tracklet->fpgaphi0().value();
0650 
0651   int itseed = tracklet->fpgat().value();
0652   int iz0seed = tracklet->fpgaz0().value();
0653 
0654   int irinvfit = irinvseed + ((idrinv + (1 << settings_.fitrinvbitshift())) >> settings_.fitrinvbitshift());
0655 
0656   int iphi0fit = iphi0seed + (idphi0 >> settings_.fitphi0bitshift());
0657   int itfit = itseed + (idt >> settings_.fittbitshift());
0658 
0659   int iz0fit = iz0seed + (idz0 >> settings_.fitz0bitshift());
0660 
0661   double rinvfit = rinvseed - drinv;
0662   double phi0fit = phi0seed - dphi0;
0663 
0664   double tfit = tseed - dt;
0665   double z0fit = z0seed - dz0;
0666 
0667   double rinvfitexact = rinvseedexact - drinvexact;
0668   double phi0fitexact = phi0seedexact - dphi0exact;
0669 
0670   double tfitexact = tseedexact - dtexact;
0671   double z0fitexact = z0seedexact - dz0exact;
0672 
0673   double chisqfitexact = chisqseedexact + deltaChisqexact;
0674 
0675   ////////////// NEW CHISQ /////////////////////
0676   bool NewChisqDebug = false;
0677   double chisqfit = 0.0;
0678   uint ichisqfit = 0;
0679 
0680   double phifactor;
0681   double rzfactor;
0682   double iphifactor;
0683   double irzfactor;
0684   int k = 0;  // column index of D matrix
0685 
0686   if (NewChisqDebug) {
0687     edm::LogVerbatim("Tracklet") << "OG chisq: \n"
0688                                  << "drinv/cov = " << drinv << "/" << drinv_cov << " \n"
0689                                  << "dphi0/cov = " << drinv << "/" << dphi0_cov << " \n"
0690                                  << "dt/cov = " << drinv << "/" << dt_cov << " \n"
0691                                  << "dz0/cov = " << drinv << "/" << dz0_cov << "\n";
0692     std::string myout = "D[0][k]= ";
0693     for (unsigned int i = 0; i < 2 * n; i++) {
0694       myout += std::to_string(D[0][i]);
0695       myout += ", ";
0696     }
0697     edm::LogVerbatim("Tracklet") << myout;
0698   }
0699 
0700   for (unsigned int i = 0; i < n; i++) {  // loop over stubs
0701 
0702     phifactor = rstub[k / 2] * delta[k] / sigma[k] + D[0][k] * drinv + D[1][k] * dphi0 + D[2][k] * dt + D[3][k] * dz0;
0703     iphifactor = kfactor[k] * rstub[k / 2] * idelta[k] * (1 << settings_.chisqphifactbits()) / sigma[k] -
0704                  iD[0][k] * idrinv - iD[1][k] * idphi0 - iD[2][k] * idt - iD[3][k] * idz0;
0705 
0706     if (NewChisqDebug) {
0707       edm::LogVerbatim("Tracklet") << "delta[k]/sigma = " << delta[k] / sigma[k] << "  delta[k] = " << delta[k] << "\n"
0708                                    << "sum = " << phifactor - delta[k] / sigma[k] << "  drinvterm = " << D[0][k] * drinv
0709                                    << "  dphi0term = " << D[1][k] * dphi0 << "  dtterm = " << D[2][k] * dt
0710                                    << "  dz0term = " << D[3][k] * dz0 << "\n  phifactor = " << phifactor;
0711     }
0712 
0713     chisqfit += phifactor * phifactor;
0714     ichisqfit += iphifactor * iphifactor / (1 << (2 * settings_.chisqphifactbits() - 4));
0715 
0716     k++;
0717 
0718     rzfactor = delta[k] / sigma[k] + D[0][k] * drinv + D[1][k] * dphi0 + D[2][k] * dt + D[3][k] * dz0;
0719     irzfactor = kfactor[k] * idelta[k] * (1 << settings_.chisqzfactbits()) / sigma[k] - iD[0][k] * idrinv -
0720                 iD[1][k] * idphi0 - iD[2][k] * idt - iD[3][k] * idz0;
0721 
0722     if (NewChisqDebug) {
0723       edm::LogVerbatim("Tracklet") << "delta[k]/sigma = " << delta[k] / sigma[k] << "  delta[k] = " << delta[k] << "\n"
0724                                    << "sum = " << rzfactor - delta[k] / sigma[k] << "  drinvterm = " << D[0][k] * drinv
0725                                    << "  dphi0term = " << D[1][k] * dphi0 << "  dtterm = " << D[2][k] * dt
0726                                    << "  dz0term = " << D[3][k] * dz0 << "\n  rzfactor = " << rzfactor;
0727     }
0728 
0729     chisqfit += rzfactor * rzfactor;
0730     ichisqfit += irzfactor * irzfactor / (1 << (2 * settings_.chisqzfactbits() - 4));
0731 
0732     k++;
0733   }
0734 
0735   if (settings_.writeMonitorData("ChiSq")) {
0736     globals_->ofstream("chisq.txt") << asinh(itfit * settings_.ktpars()) << " " << chisqfit << " " << ichisqfit / 16.0
0737                                     << endl;
0738   }
0739 
0740   // Chisquare per DOF capped out at 11 bits, so 15 is an educated guess
0741   if (ichisqfit >= (1 << 15)) {
0742     if (NewChisqDebug) {
0743       edm::LogVerbatim("Tracklet") << "CHISQUARE (" << ichisqfit << ") LARGER THAN 11 BITS!";
0744     }
0745     ichisqfit = (1 << 15) - 1;
0746   }
0747 
0748   // Eliminate lower bits to fit in 8 bits
0749   ichisqfit = ichisqfit >> 7;
0750   // Probably redundant... enforce 8 bit cap
0751   if (ichisqfit >= (1 << 8))
0752     ichisqfit = (1 << 8) - 1;
0753 
0754   double phicrit = phi0fit - asin(0.5 * settings_.rcrit() * rinvfit);
0755   bool keep = (phicrit > settings_.phicritmin()) && (phicrit < settings_.phicritmax());
0756 
0757   if (!keep) {
0758     return;
0759   }
0760 
0761   // NOTE: setFitPars in Tracklet.h now accepts chi2 r-phi and chi2 r-z values. This class only has access
0762   // to the composite chi2. When setting fit parameters on a tracklet, this places all of the chi2 into the
0763   // r-phi fit, and sets the r-z fit value to zero.
0764   //
0765   // This is also true for the call to setFitPars in trackFitFake.
0766   tracklet->setFitPars(rinvfit,
0767                        phi0fit,
0768                        0.0,
0769                        tfit,
0770                        z0fit,
0771                        chisqfit,
0772                        0.0,
0773                        rinvfitexact,
0774                        phi0fitexact,
0775                        0.0,
0776                        tfitexact,
0777                        z0fitexact,
0778                        chisqfitexact,
0779                        0.0,
0780                        irinvfit,
0781                        iphi0fit,
0782                        0,
0783                        itfit,
0784                        iz0fit,
0785                        ichisqfit,
0786                        0,
0787                        0);
0788 }
0789 
0790 void FitTrack::trackFitFake(Tracklet* tracklet, std::vector<const Stub*>&, std::vector<std::pair<int, int>>&) {
0791   tracklet->setFitPars(tracklet->rinvapprox(),
0792                        tracklet->phi0approx(),
0793                        tracklet->d0approx(),
0794                        tracklet->tapprox(),
0795                        tracklet->z0approx(),
0796                        0.0,
0797                        0.0,
0798                        tracklet->rinv(),
0799                        tracklet->phi0(),
0800                        tracklet->d0(),
0801                        tracklet->t(),
0802                        tracklet->z0(),
0803                        0.0,
0804                        0.0,
0805                        tracklet->fpgarinv().value(),
0806                        tracklet->fpgaphi0().value(),
0807                        tracklet->fpgad0().value(),
0808                        tracklet->fpgat().value(),
0809                        tracklet->fpgaz0().value(),
0810                        0,
0811                        0,
0812                        0);
0813   return;
0814 }
0815 
0816 std::vector<Tracklet*> FitTrack::orderedMatches(vector<FullMatchMemory*>& fullmatch) {
0817   std::vector<Tracklet*> tmp;
0818 
0819   std::vector<unsigned int> indexArray;
0820   for (auto& imatch : fullmatch) {
0821     //check that we have correct order
0822     if (imatch->nMatches() > 1) {
0823       for (unsigned int j = 0; j < imatch->nMatches() - 1; j++) {
0824         assert(imatch->getTracklet(j)->TCID() <= imatch->getTracklet(j + 1)->TCID());
0825       }
0826     }
0827 
0828     if (settings_.debugTracklet() && imatch->nMatches() != 0) {
0829       edm::LogVerbatim("Tracklet") << "orderedMatches: " << imatch->getName() << " " << imatch->nMatches();
0830     }
0831 
0832     indexArray.push_back(0);
0833   }
0834 
0835   int bestIndex = -1;
0836   do {
0837     int bestTCID = -1;
0838     bestIndex = -1;
0839     for (unsigned int i = 0; i < fullmatch.size(); i++) {
0840       if (indexArray[i] >= fullmatch[i]->nMatches()) {
0841         //skip as we were at the end
0842         continue;
0843       }
0844       int TCID = fullmatch[i]->getTracklet(indexArray[i])->TCID();
0845       if (TCID < bestTCID || bestTCID < 0) {
0846         bestTCID = TCID;
0847         bestIndex = i;
0848       }
0849     }
0850     if (bestIndex != -1) {
0851       tmp.push_back(fullmatch[bestIndex]->getTracklet(indexArray[bestIndex]));
0852       indexArray[bestIndex]++;
0853     }
0854   } while (bestIndex != -1);
0855 
0856   for (unsigned int i = 0; i < tmp.size(); i++) {
0857     if (i > 0) {
0858       //This allows for equal TCIDs. This means that we can e.g. have a track seeded in L1L2 that projects to both L3 and D4.
0859       //The algorithm will pick up the first hit and drop the second.
0860       if (tmp[i - 1]->TCID() > tmp[i]->TCID()) {
0861         edm::LogVerbatim("Tracklet") << "Wrong TCID ordering in " << getName() << " : " << tmp[i - 1]->TCID() << " "
0862                                      << tmp[i]->TCID();
0863       }
0864     }
0865   }
0866 
0867   return tmp;
0868 }
0869 
0870 void FitTrack::execute(unsigned int iSector) {
0871   // merge
0872   const std::vector<Tracklet*>& matches1 = orderedMatches(fullmatch1_);
0873   const std::vector<Tracklet*>& matches2 = orderedMatches(fullmatch2_);
0874   const std::vector<Tracklet*>& matches3 = orderedMatches(fullmatch3_);
0875   const std::vector<Tracklet*>& matches4 = orderedMatches(fullmatch4_);
0876 
0877   iSector_ = iSector;
0878 
0879   if (settings_.debugTracklet() && (matches1.size() + matches2.size() + matches3.size() + matches4.size()) > 0) {
0880     for (auto& imatch : fullmatch1_) {
0881       edm::LogVerbatim("Tracklet") << imatch->getName() << " " << imatch->nMatches();
0882     }
0883     edm::LogVerbatim("Tracklet") << getName() << " matches : " << matches1.size() << " " << matches2.size() << " "
0884                                  << matches3.size() << " " << matches4.size();
0885   }
0886 
0887   unsigned int indexArray[4];
0888   for (unsigned int i = 0; i < 4; i++) {
0889     indexArray[i] = 0;
0890   }
0891 
0892   int countAll = 0;
0893   int countFit = 0;
0894 
0895   Tracklet* bestTracklet = nullptr;
0896   do {
0897     countAll++;
0898     bestTracklet = nullptr;
0899 
0900     if (indexArray[0] < matches1.size()) {
0901       if (bestTracklet == nullptr) {
0902         bestTracklet = matches1[indexArray[0]];
0903       } else {
0904         if (matches1[indexArray[0]]->TCID() < bestTracklet->TCID())
0905           bestTracklet = matches1[indexArray[0]];
0906       }
0907     }
0908 
0909     if (indexArray[1] < matches2.size()) {
0910       if (bestTracklet == nullptr) {
0911         bestTracklet = matches2[indexArray[1]];
0912       } else {
0913         if (matches2[indexArray[1]]->TCID() < bestTracklet->TCID())
0914           bestTracklet = matches2[indexArray[1]];
0915       }
0916     }
0917 
0918     if (indexArray[2] < matches3.size()) {
0919       if (bestTracklet == nullptr) {
0920         bestTracklet = matches3[indexArray[2]];
0921       } else {
0922         if (matches3[indexArray[2]]->TCID() < bestTracklet->TCID())
0923           bestTracklet = matches3[indexArray[2]];
0924       }
0925     }
0926 
0927     if (indexArray[3] < matches4.size()) {
0928       if (bestTracklet == nullptr) {
0929         bestTracklet = matches4[indexArray[3]];
0930       } else {
0931         if (matches4[indexArray[3]]->TCID() < bestTracklet->TCID())
0932           bestTracklet = matches4[indexArray[3]];
0933       }
0934     }
0935 
0936     if (bestTracklet == nullptr)
0937       break;
0938 
0939     //Counts total number of matched hits
0940     int nMatches = 0;
0941 
0942     //Counts unique hits in each layer
0943     int nMatchesUniq = 0;
0944     bool match = false;
0945 
0946     while (indexArray[0] < matches1.size() && matches1[indexArray[0]] == bestTracklet) {
0947       indexArray[0]++;
0948       nMatches++;
0949       match = true;
0950     }
0951 
0952     if (match)
0953       nMatchesUniq++;
0954     match = false;
0955 
0956     while (indexArray[1] < matches2.size() && matches2[indexArray[1]] == bestTracklet) {
0957       indexArray[1]++;
0958       nMatches++;
0959       match = true;
0960     }
0961 
0962     if (match)
0963       nMatchesUniq++;
0964     match = false;
0965 
0966     while (indexArray[2] < matches3.size() && matches3[indexArray[2]] == bestTracklet) {
0967       indexArray[2]++;
0968       nMatches++;
0969       match = true;
0970     }
0971 
0972     if (match)
0973       nMatchesUniq++;
0974     match = false;
0975 
0976     while (indexArray[3] < matches4.size() && matches4[indexArray[3]] == bestTracklet) {
0977       indexArray[3]++;
0978       nMatches++;
0979       match = true;
0980     }
0981 
0982     if (match)
0983       nMatchesUniq++;
0984 
0985     if (settings_.debugTracklet()) {
0986       edm::LogVerbatim("Tracklet") << getName() << " : nMatches = " << nMatches << " nMatchesUniq = " << nMatchesUniq
0987                                    << " " << asinh(bestTracklet->t());
0988     }
0989 
0990     std::vector<const Stub*> trackstublist;
0991     std::vector<std::pair<int, int>> stubidslist;
0992     if ((bestTracklet->getISeed() >= (int)N_SEED_PROMPT && nMatchesUniq >= 1) ||
0993         nMatchesUniq >= 2) {  //For seeds index >=8 (triplet seeds), there are three stubs associated from start.
0994       countFit++;
0995 
0996 #ifdef USEHYBRID
0997       trackFitKF(bestTracklet, trackstublist, stubidslist);
0998 #else
0999       if (settings_.fakefit()) {
1000         trackFitFake(bestTracklet, trackstublist, stubidslist);
1001       } else {
1002         trackFitChisq(bestTracklet, trackstublist, stubidslist);
1003       }
1004 #endif
1005 
1006       if (settings_.removalType() == "merge") {
1007         trackfit_->addStubList(trackstublist);
1008         trackfit_->addStubidsList(stubidslist);
1009         bestTracklet->setTrackIndex(trackfit_->nTracks());
1010         trackfit_->addTrack(bestTracklet);
1011       } else if (bestTracklet->fit()) {
1012         assert(trackfit_ != nullptr);
1013         if (settings_.writeMonitorData("Seeds")) {
1014           ofstream fout("seeds.txt", ofstream::app);
1015           fout << __FILE__ << ":" << __LINE__ << " " << name_ << "_"
1016                << " " << bestTracklet->getISeed() << endl;
1017           fout.close();
1018         }
1019         bestTracklet->setTrackIndex(trackfit_->nTracks());
1020         trackfit_->addTrack(bestTracklet);
1021       }
1022     }
1023 
1024   } while (bestTracklet != nullptr);
1025 
1026   if (settings_.writeMonitorData("FT")) {
1027     globals_->ofstream("fittrack.txt") << getName() << " " << countAll << " " << countFit << endl;
1028   }
1029 }