L1TMuonBarrelKalmanAlgo.cc

Go to the documentation of this file.

#include <cmath>
#include "L1Trigger/L1TMuonBarrel/interface/L1TMuonBarrelKalmanAlgo.h"


L1TMuonBarrelKalmanAlgo::L1TMuonBarrelKalmanAlgo(const edm::ParameterSet& settings):
  verbose_(settings.getParameter<bool>("verbose")),
  lutService_(new L1TMuonBarrelKalmanLUTs(settings.getParameter<std::string>("lutFile"))),
  initK_(settings.getParameter<std::vector<double> >("initialK")),
  initK2_(settings.getParameter<std::vector<double> >("initialK2")),
  eLoss_(settings.getParameter<std::vector<double> >("eLoss")),
  aPhi_(settings.getParameter<std::vector<double> >("aPhi")),
  aPhiB_(settings.getParameter<std::vector<double> >("aPhiB")),
  aPhiBNLO_(settings.getParameter<std::vector<double> >("aPhiBNLO")),
  bPhi_(settings.getParameter<std::vector<double> >("bPhi")),
  bPhiB_(settings.getParameter<std::vector<double> >("bPhiB")),
  phiAt2_(settings.getParameter<double>("phiAt2")),

  chiSquare_(settings.getParameter<std::vector<double> >("chiSquare")),
  chiSquareCutPattern_(settings.getParameter<std::vector<int> >("chiSquareCutPattern")),
  chiSquareCutCurv_(settings.getParameter<std::vector<int> >("chiSquareCutCurvMax")),
  chiSquareCut_(settings.getParameter<std::vector<int> >("chiSquareCut")),
  trackComp_(settings.getParameter<std::vector<double> >("trackComp")),
  trackCompErr1_(settings.getParameter<std::vector<double> >("trackCompErr1")),
  trackCompErr2_(settings.getParameter<std::vector<double> >("trackCompErr2")),
  trackCompPattern_(settings.getParameter<std::vector<int> >("trackCompCutPattern")),
  trackCompCutCurv_(settings.getParameter<std::vector<int> >("trackCompCutCurvMax")),
  trackCompCut_(settings.getParameter<std::vector<int> >("trackCompCut")),
  chiSquareCutTight_(settings.getParameter<std::vector<int> >("chiSquareCutTight")),
  combos4_(settings.getParameter<std::vector<int> >("combos4")),
  combos3_(settings.getParameter<std::vector<int> >("combos3")),
  combos2_(settings.getParameter<std::vector<int> >("combos2")),
  combos1_(settings.getParameter<std::vector<int> >("combos1")),
  useOfflineAlgo_(settings.getParameter<bool>("useOfflineAlgo")),
  mScatteringPhi_(settings.getParameter<std::vector<double> >("mScatteringPhi")),
  mScatteringPhiB_(settings.getParameter<std::vector<double> >("mScatteringPhiB")),
  pointResolutionPhi_(settings.getParameter<double>("pointResolutionPhi")),
  pointResolutionPhiB_(settings.getParameter<double>("pointResolutionPhiB")),
  pointResolutionVertex_(settings.getParameter<double>("pointResolutionVertex"))  

{



}



std::pair<bool,uint> L1TMuonBarrelKalmanAlgo::getByCode(const L1MuKBMTrackCollection& tracks,int mask) {
  for (uint i=0;i<tracks.size();++i) {
    printf("Code=%d, track=%d\n",tracks[i].hitPattern(),mask); 
    if (tracks[i].hitPattern()==mask)
      return std::make_pair(true,i);
  }
  return std::make_pair(false,0);
}


l1t::RegionalMuonCand  
L1TMuonBarrelKalmanAlgo::convertToBMTF(const L1MuKBMTrack& track) {
  //  int  K = fabs(track.curvatureAtVertex());

  //calibration
  int sign,signValid;

  if (track.curvatureAtVertex()==0) {
    sign=0;
    signValid=0;
  }
  else if (track.curvatureAtVertex()>0) {
    sign=0;
    signValid=1;
  }
  else  {
    sign=1;
    signValid=1;
  }   

  // if (K<22)
  //   K=22;

  // if (K>4095)
  //   K=4095;

  int pt = ptLUT(track.curvatureAtVertex());


  // int  K2 = fabs(track.curvatureAtMuon());
  // if (K2<22)
  //   K2=22;

  // if (K2>4095)
  //   K2=4095;
  int pt2 = ptLUT(track.curvatureAtMuon())/2;
  int eta  = track.hasFineEta() ? track.fineEta() : track.coarseEta();


  //  int phi2 = track.phiAtMuon()>>2;
  //  float phi_f = float(phi2);
  //double kPhi = 57.2958/0.625/1024.;
  //int phi = 24+int(floor(kPhi*phi_f));
  //  if (phi >  69) phi =  69;
  //  if (phi < -8) phi = -8;
  int phi2 = track.phiAtMuon()>>2;
  int tmp = fp_product(0.0895386,phi2,14);
  int phi = 24+tmp;





  int processor=track.sector();
  int HF = track.hasFineEta();
  
  int quality=12|(rank(track)>>6);

  int dxy=abs(track.dxy())>>9;
  if (dxy>3)
    dxy=3;

  int trackAddr;
  std::map<int,int> addr = trackAddress(track,trackAddr);

  l1t::RegionalMuonCand muon(pt,phi,eta,sign,signValid,quality,processor,l1t::bmtf,addr);
  muon.setHwHF(HF);
  muon.setHwPt2(pt2);
  muon.setHwDXY(dxy);


  //nw the words!
  uint32_t word1=pt;
  word1=word1 | quality<<9;
  word1=word1 | (twosCompToBits(eta))<<13;
  word1=word1 | HF<<22;
  word1=word1 | (twosCompToBits(phi))<<23;

  uint32_t word2=sign;
  word2=word2 | signValid<<1;
  word2=word2 | dxy<<2;
  word2=word2 | trackAddr<<4;
  word2=word2 | (twosCompToBits(track.wheel()))<<20;
  word2=word2 | pt2<<23;
  muon.setDataword(word2,word1);
  return muon;
}

void L1TMuonBarrelKalmanAlgo::addBMTFMuon(int bx,const L1MuKBMTrack& track,  std::unique_ptr<l1t::RegionalMuonCandBxCollection>& out) { 
  out->push_back(bx,convertToBMTF(track));
}



// std::pair<bool,uint> L1TMuonBarrelKalmanAlgo::match(const L1MuKBMTCombinedStubRef& seed, const L1MuKBMTCombinedStubRefVector& stubs,int step) {
//   L1MuKBMTCombinedStubRefVector selected;

//   bool found=false;
//   uint best=0;
//   int distance=100000;
//   uint N=0;
//   for (const auto& stub :stubs)  {
//     N=N+1;
//     if (stub->stNum()!=step) 
//       continue;

//     int d = fabs(wrapAround(((correctedPhi(seed,seed->scNum())-correctedPhi(stub,seed->scNum()))>>3),1024));
//     if (d<distance) {
//       distance = d;
//       best=N-1;
//       found=true;
//     }
//   }
//   return std::make_pair(found,best);
// }



uint L1TMuonBarrelKalmanAlgo::matchAbs(std::map<uint,uint>& info, uint i, uint j) {
  if (info[i]<info[j])
    return i;
  else
    return j;
}


std::pair<bool,uint> L1TMuonBarrelKalmanAlgo::match(const L1MuKBMTCombinedStubRef& seed, const L1MuKBMTCombinedStubRefVector& stubs,int step) {
  L1MuKBMTCombinedStubRefVector selected;

  std::map<uint,uint> diffInfo;
  for (uint i=0;i<12;++i) {
    diffInfo[i]=60000;
  }

  std::map<uint,uint> stubInfo;

  int sector = seed->scNum();
  int previousSector=sector-1;
  int nextSector=sector+1;
  if (sector==0) {
    previousSector=11;
  }
  if (sector==11) {
    nextSector=0;
  }

  int wheel = seed->whNum();
  int innerWheel=0;
  if (wheel==-2)
    innerWheel=-1;
  if (wheel==-1)
    innerWheel=0;
  if (wheel==0)
    innerWheel=1982;
  if (wheel==1)
    innerWheel=0;
  if (wheel==2)
    innerWheel=1;


  //First align the data 
  uint N=0;
  for (const auto& stub :stubs)  {
    N=N+1;

    if (stub->stNum()!=step) 
      continue;

    uint distance = fabs(wrapAround(((correctedPhi(seed,seed->scNum())-correctedPhi(stub,seed->scNum()))>>3),1024));

    if (stub->scNum()==previousSector) {
      if (stub->whNum()==wheel) {
    if (!stub->tag()) {
      diffInfo[0]=distance;
      stubInfo[0]=N-1;
    }
    else {
      diffInfo[1]=distance;
      stubInfo[1]=N-1;
    }
      }
      else if (stub->whNum()==innerWheel){
    if (!stub->tag()) {
      diffInfo[2]=distance;
      stubInfo[2]=N-1;
    }
    else {
      diffInfo[3]=distance;
      stubInfo[3]=N-1;
    }
      }
    }
    else  if (stub->scNum()==sector) {
      if (stub->whNum()==wheel) {
    if (!stub->tag()) {
      diffInfo[4]=distance;
      stubInfo[4]=N-1;
    }
    else {
      diffInfo[5]=distance;
      stubInfo[5]=N-1;
    }
      }
      else if (stub->whNum()==innerWheel){
    if (!stub->tag()) {
      diffInfo[6]=distance;
      stubInfo[6]=N-1;
    }
    else {
      diffInfo[7]=distance;
      stubInfo[7]=N-1;
    }
      }
    }
    else  if (stub->scNum()==nextSector) {
      if (stub->whNum()==wheel) {
    if (!stub->tag()) {
      diffInfo[8]=distance;
      stubInfo[8]=N-1;
    }
    else {
      diffInfo[9]=distance;
      stubInfo[9]=N-1;
    }
      }
      else if (stub->whNum()==innerWheel){
    if (!stub->tag()) {
      diffInfo[10]=distance;
      stubInfo[10]=N-1;
    }
    else {
      diffInfo[11]=distance;
      stubInfo[11]=N-1;
    }
      }
    }

  }


  uint s1_1 = matchAbs(diffInfo,0,1);
  uint s1_2 = matchAbs(diffInfo,2,3);
  uint s1_3 = matchAbs(diffInfo,4,5);
  uint s1_4 = matchAbs(diffInfo,6,7);
  uint s1_5 = matchAbs(diffInfo,8,9);
  uint s1_6 = matchAbs(diffInfo,10,11);

  uint s2_1 = matchAbs(diffInfo,s1_1,s1_2);
  uint s2_2 = matchAbs(diffInfo,s1_3,s1_4);
  uint s2_3 = matchAbs(diffInfo,s1_5,s1_6);

  uint s3_1 = matchAbs(diffInfo,s2_1,s2_2);

  uint s4 = matchAbs(diffInfo,s3_1,s2_3);

  

  if (diffInfo[s4]!=60000)
    return std::make_pair(true,stubInfo[s4]);
  else
    return std::make_pair(false,0);
}






int L1TMuonBarrelKalmanAlgo::correctedPhiB(const L1MuKBMTCombinedStubRef& stub) {
  //Promote phiB to 12 bits
  return 8*stub->phiB();

}

int L1TMuonBarrelKalmanAlgo::correctedPhi(const L1MuKBMTCombinedStubRef& stub,int sector) {
  if (stub->scNum()==sector) {
    return stub->phi();
  }
  else if ((stub->scNum()==sector-1) || (stub->scNum()==11 && sector==0)) {
    return stub->phi()-2144;
  }
  else if ((stub->scNum()==sector+1) || (stub->scNum()==0 && sector==11)) {
    return stub->phi()+2144;
  }
  return stub->phi();
} 


int L1TMuonBarrelKalmanAlgo::hitPattern(const L1MuKBMTrack& track)  {
  unsigned int mask = 0;
  for (const auto& stub : track.stubs()) {
    mask = mask+round(pow(2,stub->stNum()-1));
  }
  return mask;
}



int L1TMuonBarrelKalmanAlgo::customBitmask(unsigned int bit1,unsigned int bit2,unsigned int bit3,unsigned int bit4)  {
  return bit1*1+bit2*2+bit3*4+bit4*8;
}

bool L1TMuonBarrelKalmanAlgo::getBit(int bitmask,int pos)  {
  return (bitmask & ( 1 << pos )) >> pos;
}



void L1TMuonBarrelKalmanAlgo::propagate(L1MuKBMTrack& track) {
  int K = track.curvature();
  int phi = track.positionAngle();
  int phiB = track.bendingAngle();
  unsigned int step = track.step();

  //energy loss term only for MU->VERTEX
  //int offset=int(charge*eLoss_[step-1]*K*K);
  //  if (fabs(offset)>4096)
  //      offset=4096*offset/fabs(offset);

  int charge=1;
  if (K!=0) 
    charge = K/fabs(K);




  int KBound=K;
  if (KBound>4095)
    KBound=4095;
  if (KBound<-4095)
    KBound=-4095;

  int deltaK=0;
  int KNew=0;
  if (step==1) {
    int addr = KBound/2;
    if (addr<0) 
      addr=(-KBound)/2;
    deltaK =2*addr-int(2*addr/(1+eLoss_[step-1]*addr));
  
    if (verbose_)
      printf("propagate to vertex K=%d deltaK=%d addr=%d\n",K,deltaK,addr);
  }

  if (K>=0)
    KNew=K-deltaK;
  else 
    KNew=K+deltaK;


  //phi propagation
  int phi11 = fp_product(aPhi_[step-1],K,10);
  int phi12 = fp_product(-bPhi_[step-1],phiB,10);
  if (verbose_) {
    printf("phi prop = %d* %f = %d, %d* %f =%d\n",K,aPhi_[step-1],phi11,phiB,-bPhi_[step-1],phi12);
  }
  int phiNew =wrapAround(phi+phi11+phi12,2048);
  //phiB propagation
  int phiB11 = fp_product(aPhiB_[step-1],K,10);
  int phiB12 = fp_product(bPhiB_[step-1],phiB,11);
  int phiBNew = wrapAround(phiB11+phiB12,4096);
  if (verbose_) {
    printf("phiB prop = %d* %f = %d, %d* %f =%d\n",K,aPhiB_[step-1],phiB11,phiB,bPhiB_[step-1],phiB12);
  }

  
  //Only for the propagation to vertex we use the LUT for better precision and the full function
  if (step==1) {
    int addr = KBound/2;
    phiBNew = wrapAround(int(aPhiB_[step-1]*addr/(1+charge*aPhiBNLO_[step-1]*addr))+int(bPhiB_[step-1]*phiB),4096);
  }
  //Rest of the stuff  is for the offline version only 
  //where we want to check what is happening in the covariaznce matrix 
  
  //Create the transformation matrix
  double a[9];
  a[0] = 1.;
  a[1] = 0.0;
  a[2] = 0.0;
  a[3] = aPhi_[step-1];
  //  a[3] = 0.0;
  a[4] = 1.0;
  a[5] = -bPhi_[step-1];
  //a[6]=0.0;
  a[6] = aPhiB_[step-1];
  if (step==1)
    a[6] = aPhiB_[step-1]/2.0;

  a[7] = 0.0;
  a[8] = bPhiB_[step-1];


  ROOT::Math::SMatrix<double,3> P(a,9);

  const std::vector<double>& covLine = track.covariance();
  L1MuKBMTrack::CovarianceMatrix cov(covLine.begin(),covLine.end());
  cov = ROOT::Math::Similarity(P,cov);

  
  //Add the multiple scattering
  double phiRMS = mScatteringPhi_[step-1]*K*K;
  double phiBRMS = mScatteringPhiB_[step-1]*K*K;

  std::vector<double> b(6);
  b[0] = 0;
  b[1] = 0;
  b[2] =phiRMS;
  b[3] =0;
  b[4] = 0;
  b[5] = phiBRMS;

  reco::Candidate::CovarianceMatrix MS(b.begin(),b.end());

  cov = cov+MS;

  if (verbose_) {
    printf("Covariance term for phiB = %f\n",cov(2,2));
    printf("Multiple scattering term for phiB = %f\n",MS(2,2));
  }
 


  track.setCovariance(cov);
  track.setCoordinates(step-1,KNew,phiNew,phiBNew);

}


bool L1TMuonBarrelKalmanAlgo::update(L1MuKBMTrack& track,const L1MuKBMTCombinedStubRef& stub,int mask) {
  updateEta(track,stub);
  if (useOfflineAlgo_) {
    if (mask==3 || mask ==5 || mask==9 ||mask==6|| mask==10 ||mask==12)
          return updateOffline(track,stub);
        else
      return updateOffline1D(track,stub);

  }
  else
    return updateLUT(track,stub,mask);

}

bool L1TMuonBarrelKalmanAlgo::updateOffline(L1MuKBMTrack& track,const L1MuKBMTCombinedStubRef& stub) {
    int trackK = track.curvature();
    int trackPhi = track.positionAngle();
    int trackPhiB = track.bendingAngle();

    int phi  = correctedPhi(stub,track.sector());
    int phiB = correctedPhiB(stub);




    Vector2 residual;
    residual[0] = phi-trackPhi;
    residual[1] = phiB-trackPhiB;

   
   


    Matrix23 H;
    H(0,0)=0.0;
    H(0,1)=1.0;
    H(0,2)=0.0;
    H(1,0)=0.0;
    H(1,1)=0.0;
    H(1,2)=1.0;

    
    CovarianceMatrix2 R;
    R(0,0) = pointResolutionPhi_;
    R(0,1) = 0.0;
    R(1,0) = 0.0;
    R(1,1) = pointResolutionPhiB_;

    const std::vector<double>& covLine = track.covariance();
    L1MuKBMTrack::CovarianceMatrix cov(covLine.begin(),covLine.end());


    CovarianceMatrix2 S = ROOT::Math::Similarity(H,cov)+R;
    if (!S.Invert())
      return false;
    Matrix32 Gain = cov*ROOT::Math::Transpose(H)*S;

    track.setKalmanGain(track.step(),fabs(trackK),Gain(0,0),Gain(0,1),Gain(1,0),Gain(1,1),Gain(2,0),Gain(2,1));

    int KNew  = (trackK+int(Gain(0,0)*residual(0)+Gain(0,1)*residual(1)));
    if (fabs(KNew)>8192)
      return false;
    
    int phiNew  = wrapAround(trackPhi+residual(0),8192);
    int phiBNew = wrapAround(trackPhiB+int(Gain(2,0)*residual(0)+Gain(2,1)*residual(1)),4096);
    
    track.setResidual(stub->stNum()-1,fabs(phi-phiNew)+fabs(phiB-phiBNew)/8);


    if (verbose_) {
      printf(" K = %d + %f * %f + %f * %f\n",trackK,Gain(0,0),residual(0),Gain(0,1),residual(1));
      printf(" phiB = %d + %f * %f + %f * %f\n",trackPhiB,Gain(2,0),residual(0),Gain(2,1),residual(1));
    }


    track.setCoordinates(track.step(),KNew,phiNew,phiBNew);
    Matrix33 covNew = cov - Gain*(H*cov);
    L1MuKBMTrack::CovarianceMatrix c;
 
    c(0,0)=covNew(0,0); 
    c(0,1)=covNew(0,1); 
    c(0,2)=covNew(0,2); 
    c(1,0)=covNew(1,0); 
    c(1,1)=covNew(1,1); 
    c(1,2)=covNew(1,2); 
    c(2,0)=covNew(2,0); 
    c(2,1)=covNew(2,1); 
    c(2,2)=covNew(2,2); 
    if (verbose_) {
      printf("Post Fit Covariance Matrix %f %f %f \n",cov(0,0),cov(1,1),cov(2,2));
      
    }

    track.setCovariance(c);
    track.addStub(stub);
    track.setHitPattern(hitPattern(track));

    return true;
}


bool L1TMuonBarrelKalmanAlgo::updateOffline1D(L1MuKBMTrack& track,const L1MuKBMTCombinedStubRef& stub) {
    int trackK = track.curvature();
    int trackPhi = track.positionAngle();
    int trackPhiB = track.bendingAngle();


    int phi  = correctedPhi(stub,track.sector());


    double residual= phi-trackPhi;

    Matrix13 H;
    H(0,0)=0.0;
    H(0,1)=1.0;
    H(0,2)=0.0;
    

    const std::vector<double>& covLine = track.covariance();
    L1MuKBMTrack::CovarianceMatrix cov(covLine.begin(),covLine.end());

    double S = ROOT::Math::Similarity(H,cov)(0,0)+pointResolutionPhi_;

    if (S==0.0)
      return false;
    Matrix31 Gain = cov*ROOT::Math::Transpose(H)/S;

    track.setKalmanGain(track.step(),fabs(trackK),Gain(0,0),0.0,Gain(1,0),0.0,Gain(2,0),0.0);

    int KNew  = wrapAround(trackK+int(Gain(0,0)*residual),8192);
    int phiNew  = wrapAround(trackPhi+residual,8192);
    int phiBNew = wrapAround(trackPhiB+int(Gain(2,0)*residual),4096);
    track.setCoordinates(track.step(),KNew,phiNew,phiBNew);
    Matrix33 covNew = cov - Gain*(H*cov);
    L1MuKBMTrack::CovarianceMatrix c;
 
    c(0,0)=covNew(0,0); 
    c(0,1)=covNew(0,1); 
    c(0,2)=covNew(0,2); 
    c(1,0)=covNew(1,0); 
    c(1,1)=covNew(1,1); 
    c(1,2)=covNew(1,2); 
    c(2,0)=covNew(2,0); 
    c(2,1)=covNew(2,1); 
    c(2,2)=covNew(2,2); 
    track.setCovariance(c);
    track.addStub(stub);
    track.setHitPattern(hitPattern(track));

    return true;
}



bool L1TMuonBarrelKalmanAlgo::updateLUT(L1MuKBMTrack& track,const L1MuKBMTCombinedStubRef& stub,int mask) {
    int trackK = track.curvature();
    int trackPhi = track.positionAngle();
    int trackPhiB = track.bendingAngle();


    int phi  = correctedPhi(stub,track.sector());
    int phiB = correctedPhiB(stub);

    if (stub->quality()<4)
      phiB=trackPhiB;

    Vector2 residual;
    int residualPhi = wrapAround(phi-trackPhi,4096);
    int residualPhiB = wrapAround(phiB-trackPhiB,8192);


    if (verbose_)
      printf("residuals %d-%d=%d %d-%d=%d\n",phi,trackPhi,int(residualPhi),phiB,trackPhiB,int(residualPhiB));


    uint absK = fabs(trackK);
    if (absK>4095)
      absK = 4095;

    std::vector<float> GAIN;
    //For the three stub stuff use only gains 0 and 4
    if (!(mask==3 || mask ==5 || mask==9 ||mask==6|| mask==10 ||mask==12))  {
      GAIN = lutService_->trackGain(track.step(),track.hitPattern(),absK/4);
      GAIN[1]=0.0;
      GAIN[3]=0.0;

    }
    else {
      GAIN = lutService_->trackGain2(track.step(),track.hitPattern(),absK/8);


    }
    if (verbose_)
      printf("Gains:%d  %f %f %f %f\n",absK/4,GAIN[0],GAIN[1],GAIN[2],GAIN[3]);
    track.setKalmanGain(track.step(),fabs(trackK),GAIN[0],GAIN[1],1,0,GAIN[2],GAIN[3]);


    int k_0 = fp_product(GAIN[0],residualPhi,3);
    int k_1 = fp_product(GAIN[1],residualPhiB,5);
    int KNew  = trackK+k_0+k_1;
    if (fabs(KNew)>=8191)
      return false;
    KNew = wrapAround(KNew,8192);
    int phiNew  = phi;

    //different products for different firmware logic
    int pbdouble_0 = fp_product(fabs(GAIN[2]),residualPhi,9);
    int pb_0 = fp_product(GAIN[2],residualPhi,9);
    int pb_1 = fp_product(GAIN[3],residualPhiB,9);

    if (verbose_)
      printf("phiupdate: %d %d\n",pb_0,pb_1);

    int phiBNew;
    if (!(mask==3 || mask ==5 || mask==9 ||mask==6|| mask==10 ||mask==12))  {
      phiBNew = wrapAround(trackPhiB+pb_0,4096);
      if (fabs(trackPhiB+pb_0)>=4095)
    return false;
    }
    else {
      phiBNew = wrapAround(trackPhiB+pb_1-pbdouble_0,4096);
      if (fabs(trackPhiB+pb_1-pbdouble_0)>=4095)
    return false;

    }
    track.setCoordinates(track.step(),KNew,phiNew,phiBNew);
    track.addStub(stub);
    track.setHitPattern(hitPattern(track));
    return true;
}




void L1TMuonBarrelKalmanAlgo::updateEta(L1MuKBMTrack& track,const L1MuKBMTCombinedStubRef& stub) {




}






void L1TMuonBarrelKalmanAlgo::vertexConstraint(L1MuKBMTrack& track) {
  if (useOfflineAlgo_)
    vertexConstraintOffline(track);
  else
    vertexConstraintLUT(track);
 
}


void L1TMuonBarrelKalmanAlgo::vertexConstraintOffline(L1MuKBMTrack& track) {
  double residual = -track.dxy();
  Matrix13 H;
  H(0,0)=0;
  H(0,1)=0;
  H(0,2)=1;

  const std::vector<double>& covLine = track.covariance();
  L1MuKBMTrack::CovarianceMatrix cov(covLine.begin(),covLine.end());
  
  double S = (ROOT::Math::Similarity(H,cov))(0,0)+pointResolutionVertex_;
  S=1.0/S;
  Matrix31 Gain = cov*(ROOT::Math::Transpose(H))*S;
  track.setKalmanGain(track.step(),fabs(track.curvature()),Gain(0,0),Gain(1,0),Gain(2,0));

  if (verbose_) {
    printf("sigma3=%f sigma6=%f\n",cov(0,3),cov(3,3));
    printf(" K = %d + %f * %f\n",track.curvature(),Gain(0,0),residual);
  }

  int KNew = wrapAround(int(track.curvature()+Gain(0,0)*residual),8192);
  int phiNew = wrapAround(int(track.positionAngle()+Gain(1,0)*residual),8192);
  int dxyNew = wrapAround(int(track.dxy()+Gain(2,0)*residual),8192);
  if (verbose_)
    printf("Post fit impact parameter=%d\n",dxyNew);
  track.setCoordinatesAtVertex(KNew,phiNew,-residual);
  Matrix33 covNew = cov - Gain*(H*cov);
  L1MuKBMTrack::CovarianceMatrix c;
  c(0,0)=covNew(0,0); 
  c(0,1)=covNew(0,1); 
  c(0,2)=covNew(0,2); 
  c(1,0)=covNew(1,0); 
  c(1,1)=covNew(1,1); 
  c(1,2)=covNew(1,2); 
  c(2,0)=covNew(2,0); 
  c(2,1)=covNew(2,1); 
  c(2,2)=covNew(2,2); 
  track.setCovariance(c);
  //  track.covariance = track.covariance - Gain*H*track.covariance;
}



void L1TMuonBarrelKalmanAlgo::vertexConstraintLUT(L1MuKBMTrack& track) {
  double residual = -track.dxy();
  uint absK = fabs(track.curvature());
  if (absK>2047)
    absK = 2047;

std::pair<float,float> GAIN = lutService_->vertexGain(track.hitPattern(),absK/2);
  track.setKalmanGain(track.step(),fabs(track.curvature()),GAIN.first,GAIN.second,-1);

  int k_0 = fp_product(GAIN.first,int(residual),7);
  int KNew = wrapAround(track.curvature()+k_0,8192);

  if (verbose_) {
    printf("VERTEX GAIN(%d)= %f * %d  = %d\n",absK/2,GAIN.first,int(residual),k_0);

  }


  int p_0 = fp_product(GAIN.second,int(residual),7);
  int phiNew = wrapAround(track.positionAngle()+p_0,8192);
  track.setCoordinatesAtVertex(KNew,phiNew,-residual);
}



void L1TMuonBarrelKalmanAlgo::setFloatingPointValues(L1MuKBMTrack& track,bool vertex) {
  int K,etaINT;

  if (track.hasFineEta())
    etaINT=track.fineEta();
  else
    etaINT=track.coarseEta();


  double lsb = 1.25/float(1 << 13);
  double lsbEta = 0.010875;

  

  if (vertex) {
    int charge=1;
    if (track.curvatureAtVertex()<0)
      charge=-1;
    double pt = double(ptLUT(track.curvatureAtVertex()))/2.0;


    double phi= track.sector()*M_PI/6.0+track.phiAtVertex()*M_PI/(6*2048.)-2*M_PI;

    double eta = etaINT*lsbEta;
    track.setPtEtaPhi(pt,eta,phi);
    track.setCharge(charge);
  }
  else {
    K=track.curvatureAtMuon();
    if (K==0)
      K=1;

    if (fabs(K)<46)
      K=46*K/fabs(K);
    double pt = 1.0/(lsb*fabs(K));
    if (pt<1.6)
      pt=1.6;
    track.setPtUnconstrained(pt);
  }
}



std::pair<bool,L1MuKBMTrack> L1TMuonBarrelKalmanAlgo::chain(const L1MuKBMTCombinedStubRef& seed, const L1MuKBMTCombinedStubRefVector& stubs) {
  L1MuKBMTrackCollection pretracks; 
  std::vector<int> combinatorics;
  switch(seed->stNum()) {
  case 1:
    combinatorics=combos1_;
    break;
  case 2:
    combinatorics=combos2_;
    break;

  case 3:
    combinatorics=combos3_;
    break;

  case 4:
    combinatorics=combos4_;
    break;

  default:
    printf("Something really bad happend\n");
  }

  L1MuKBMTrack nullTrack(seed,correctedPhi(seed,seed->scNum()),correctedPhiB(seed));

  for( const auto& mask : combinatorics) {
    L1MuKBMTrack track(seed,correctedPhi(seed,seed->scNum()),correctedPhiB(seed));
    int phiB = correctedPhiB(seed);
    int charge;
    if (phiB==0)
      charge = 0;
    else
      charge=phiB/fabs(phiB);

    int address=phiB;
    if (track.step()==4 && (fabs(seed->phiB())>15))
      address=charge*15*8;

    if (track.step()==3 && (fabs(seed->phiB())>30))
      address=charge*30*8;
    if (track.step()==2 && (fabs(seed->phiB())>127))
      address=charge*127*8;         
    int initialK = int(initK_[seed->stNum()-1]*address/(1+initK2_[seed->stNum()-1]*charge*address));
    if (initialK>8191)
      initialK=8191;
    if (initialK<-8191)
      initialK=-8191;

    track.setCoordinates(seed->stNum(),initialK,correctedPhi(seed,seed->scNum()),phiB);
    if (seed->quality()<4) {
      track.setCoordinates(seed->stNum(),0,correctedPhi(seed,seed->scNum()),0);     
    }



    track.setHitPattern(hitPattern(track));
    //set covariance
    L1MuKBMTrack::CovarianceMatrix covariance;  


    float DK=512*512.;
    covariance(0,0)=DK;
    covariance(0,1)=0;
    covariance(0,2)=0;
    covariance(1,0)=0;
    covariance(1,1)=float(pointResolutionPhi_);
    covariance(1,2)=0;
    covariance(2,0)=0;
    covariance(2,1)=0;
    covariance(2,2)=float(pointResolutionPhiB_);
    track.setCovariance(covariance);
    //
    if (verbose_) {
      printf("New Kalman fit staring at step=%d, phi=%d,phiB=%d with curvature=%d\n",track.step(),track.positionAngle(),track.bendingAngle(),track.curvature());
      printf("BITMASK:");
      for (unsigned int i=0;i<4;++i)
    printf("%d",getBit(mask,i));
      printf("\n");
      printf("------------------------------------------------------\n");
      printf("------------------------------------------------------\n");
      printf("------------------------------------------------------\n");
      printf("stubs:\n");
      for (const auto& stub: stubs) 
    printf("station=%d phi=%d phiB=%d qual=%d tag=%d sector=%d wheel=%d fineEta= %d %d\n",stub->stNum(),correctedPhi(stub,seed->scNum()),correctedPhiB(stub),stub->quality(),stub->tag(),stub->scNum(),stub->whNum(),stub->eta1(),stub->eta2()); 
      printf("------------------------------------------------------\n");
      printf("------------------------------------------------------\n");

    }

    int phiAtStation2=0;

    while(track.step()>0) {
      // muon station 1 
      if (track.step()==1) {
    track.setCoordinatesAtMuon(track.curvature(),track.positionAngle(),track.bendingAngle());
    phiAtStation2=phiAt2(track);
    bool passed = estimateChiSquare(track);
    if (!passed)
      break;
    calculateEta(track);
    setFloatingPointValues(track,false);
    //calculate coarse eta


    if (verbose_) 
      printf ("Unconstrained PT  in Muon System: pt=%f\n",track.ptUnconstrained());
      }
      
      propagate(track);
      if (verbose_)
    printf("propagated Coordinates step:%d,phi=%d,phiB=%d,K=%d\n",track.step(),track.positionAngle(),track.bendingAngle(),track.curvature());

      if (track.step()>0) 
    if (getBit(mask,track.step()-1)) {
      std::pair<bool,uint> bestStub = match(seed,stubs,track.step());
          if ((!bestStub.first) || (!update(track,stubs[bestStub.second],mask)))
        break;
      if (verbose_) {
        printf("updated Coordinates step:%d,phi=%d,phiB=%d,K=%d\n",track.step(),track.positionAngle(),track.bendingAngle(),track.curvature());
      }
    }
    

      if (track.step()==0) {
    track.setCoordinatesAtVertex(track.curvature(),track.positionAngle(),track.bendingAngle());
    if (verbose_)
      printf(" Coordinates before vertex constraint step:%d,phi=%d,dxy=%d,K=%d\n",track.step(),track.phiAtVertex(),track.dxy(),track.curvatureAtVertex());
    if (verbose_)
      printf("Chi Square = %d\n",track.approxChi2());

    vertexConstraint(track);
    estimateCompatibility(track);
    if (verbose_) {
      printf(" Coordinates after vertex constraint step:%d,phi=%d,dxy=%d,K=%d  maximum local chi2=%d\n",track.step(),track.phiAtVertex(),track.dxy(),track.curvatureAtVertex(),track.approxChi2());
      printf("------------------------------------------------------\n");
      printf("------------------------------------------------------\n");
    }
    setFloatingPointValues(track,true);
    //rset the coordinates at muon to include phi at station 2
    track.setCoordinatesAtMuon(track.curvatureAtMuon(),phiAtStation2,track.phiBAtMuon());
    track.setRank(rank(track));
    if (verbose_)
      printf ("Floating point coordinates at vertex: pt=%f, eta=%f phi=%f\n",track.pt(),track.eta(),track.phi());
    pretracks.push_back(track);
      }
    }
  }
  //Now for all the pretracks we need only one 
  L1MuKBMTrackCollection cleaned = clean(pretracks,seed->stNum());

  if (!cleaned.empty()) {
    return std::make_pair(true,cleaned[0]);
  }
  return std::make_pair(false,nullTrack);
}       
    





bool L1TMuonBarrelKalmanAlgo::estimateChiSquare(L1MuKBMTrack& track) {
  //here we have a simplification of the algorithm for the sake of the emulator - rsult is identical
  // we apply cuts on the firmware as |u -u'|^2 < a+b *K^2 
  int K = track.curvatureAtMuon();

  uint chi=0;

  // const double PHI[4]={0.0,0.249,0.543,0.786};
  // const double DROR[4]={0.0,0.182,0.430,0.677};



  int coords = wrapAround((track.phiAtMuon()+track.phiBAtMuon())>>4,512);
   for (const auto& stub: track.stubs()) {   
     int AK = wrapAround(fp_product(-chiSquare_[stub->stNum()-1],K>>4,8),256);
     int stubCoords =   wrapAround((correctedPhi(stub,track.sector())>>4)+(stub->phiB()>>1),512);
     int diff1 = wrapAround(stubCoords-coords,1024);
     uint delta = wrapAround(abs(diff1+AK),2048);
     chi=chi+delta;    
     if (verbose_)
       printf("Chi Square stub for track with coords=%d -> AK=%d stubCoords=%d diff=%d delta=%d\n",coords,AK,stubCoords,diff1,delta); 
   }

   if (chi>127)
     chi=127;
    // for (const auto& stub: track.stubs()) {   
    //   int deltaPhi = (correctedPhi(stub,track.sector())-track.phiAtMuon())>>3;
    //   int AK =  fp_product(PHI[stub->stNum()-1],K>>3,8);
    //   int BPB = fp_product(DROR[stub->stNum()-1],track.phiBAtMuon()>>3,8);
    //   chi=chi+abs(deltaPhi-AK-BPB);
    // }
    // //  }


  
   track.setApproxChi2(chi);

    for (uint i=0;i<chiSquareCutPattern_.size();++i)  {
      if (track.hitPattern()==chiSquareCutPattern_[i] && fabs(K)<chiSquareCutCurv_[i] && track.approxChi2()>chiSquareCut_[i])
        return false;
   }
   return true;

}



void L1TMuonBarrelKalmanAlgo::estimateCompatibility(L1MuKBMTrack& track) {
  int K = track.curvatureAtVertex()>>4;
  
  if (track.stubs().size()!=2) {
    track.setTrackCompatibility(0);
     return;
  }




  uint stubSel=1;
  if (track.stubs()[0]->quality()>track.stubs()[1]->quality())
    stubSel=0;

  const L1MuKBMTCombinedStubRef& stub = track.stubs()[stubSel];
  int absK = fabs(K);
  int diff = wrapAround(stub->phiB()-fp_product(trackComp_[stub->stNum()-1],K,9),2048);
  int delta = fabs(diff);
  uint err = wrapAround(trackCompErr1_[stub->stNum()-1]+fp_product(trackCompErr2_[stub->stNum()-1],absK,9),256);
  track.setTrackCompatibility(delta/err);

  for (uint i=0;i<trackCompPattern_.size();++i)  {
    int deltaMax = err*trackCompCut_[i];
    if ( (track.hitPattern()==trackCompPattern_[i] ) && (absK<trackCompCutCurv_[i]) && ((track.approxChi2()>chiSquareCutTight_[i]) || (delta>deltaMax)  )) {
      track.setCoordinatesAtVertex(8191,track.phiAtVertex(),track.dxy());
      break;
    }
  }

}



int L1TMuonBarrelKalmanAlgo::rank(const L1MuKBMTrack& track) {
  //    int offset=0;
  uint chi  = track.approxChi2()>127 ? 127 :track.approxChi2();
    if (hitPattern(track)==customBitmask(0,0,1,1))
      return 60;
    //    return offset+(track.stubs().size()*2+track.quality())*80-track.approxChi2();
    return 160+(track.stubs().size())*20-chi;

}



int L1TMuonBarrelKalmanAlgo::wrapAround(int value,int maximum) {

  if (value>maximum-1)
    return wrapAround(value-2*maximum,maximum);
  if (value<-maximum)
    return wrapAround(value+2*maximum,maximum);
  return value;

}



int L1TMuonBarrelKalmanAlgo::encode(bool ownwheel,int sector,bool tag) {
  if (ownwheel) {
    if (sector==0) {
      if (tag)
    return 9;
      else 
    return 8;
    }
    else if (sector==1) {
      if (tag)
    return 11;
      else 
    return 10;

    }
    else {
      if (tag)
    return 13;
      else 
    return 12;
    }

  }
  else {
    if (sector==0) {
      if (tag)
    return 1;
      else 
    return 0;
    }
    else if (sector==1) {
      if (tag)
    return 3;
      else 
    return 2;

    }
    else {
      if (tag)
    return 5;
      else 
    return 4;
    }
  }
  return 15;
} 




std::map<int,int> L1TMuonBarrelKalmanAlgo::trackAddress(const L1MuKBMTrack& track,int& word) {
  std::map<int,int> out;
  
  out[l1t::RegionalMuonCand::kWheelSide] = track.wheel()<0;
  if (track.wheel()==-2)
    out[l1t::RegionalMuonCand::kWheelNum] = 2;
  else if (track.wheel()==-1)
    out[l1t::RegionalMuonCand::kWheelNum] = 1;
  else if (track.wheel()==0)
    out[l1t::RegionalMuonCand::kWheelNum] = 0;
  else if (track.wheel()==1)
    out[l1t::RegionalMuonCand::kWheelNum] = 1;
  else if (track.wheel()==2)
    out[l1t::RegionalMuonCand::kWheelNum] = 2;
  else
    out[l1t::RegionalMuonCand::kWheelNum] = 0;
  out[l1t::RegionalMuonCand::kStat1]=3;
  out[l1t::RegionalMuonCand::kStat2]=15;
  out[l1t::RegionalMuonCand::kStat3]=15;
  out[l1t::RegionalMuonCand::kStat4]=15;
  out[l1t::RegionalMuonCand::kSegSelStat1]=0;
  out[l1t::RegionalMuonCand::kSegSelStat2]=0;
  out[l1t::RegionalMuonCand::kSegSelStat3]=0;
  out[l1t::RegionalMuonCand::kSegSelStat4]=0;
  //out[l1t::RegionalMuonCand::kNumBmtfSubAddr]=0; // This is commented out for better data/MC agreement


  for (const auto stub: track.stubs()) {
    bool ownwheel = stub->whNum() == track.wheel();
    int sector=0;
    if ((stub->scNum()==track.sector()+1) || (stub->scNum()==0 && track.sector()==11))
      sector=+1;
    if ((stub->scNum()==track.sector()-1) || (stub->scNum()==11 && track.sector()==0))
      sector=-1;
    int addr = encode(ownwheel,sector,stub->tag());
   
    if (stub->stNum()==4) {
      if (stub->tag())
    addr=1;
      else 
    addr=2;
      out[l1t::RegionalMuonCand::kStat1]=addr;
    }      
    if (stub->stNum()==3) {
      out[l1t::RegionalMuonCand::kStat2]=addr;    
    }
    if (stub->stNum()==2) {
      out[l1t::RegionalMuonCand::kStat3]=addr;
    }
    if (stub->stNum()==1) {
      out[l1t::RegionalMuonCand::kStat4]=addr;
    }
  }
    
  word=0;
  word = word | out[l1t::RegionalMuonCand::kStat1]<<12;
  word = word | out[l1t::RegionalMuonCand::kStat2]<<8;
  word = word | out[l1t::RegionalMuonCand::kStat3]<<4;
  word = word | out[l1t::RegionalMuonCand::kStat4];

  

  return out;
}



uint L1TMuonBarrelKalmanAlgo::twosCompToBits(int q) {
  if (q>=0)
    return q;
  else 
    return (~q)+1; 


}



int L1TMuonBarrelKalmanAlgo::fp_product(float a,int b, uint bits) {
  //  return long(a*(1<<bits)*b)>>bits;
  return (long((a*(1<<bits))*b))>>bits;
}



int L1TMuonBarrelKalmanAlgo::ptLUT(int K) {
  int charge = (K>=0) ? +1 : -1;
  float lsb=1.25/float(1<<13);
  float FK = fabs(K);


  if (FK>2047)
    FK=2047.;   
  if (FK<26)
    FK=26.;   

  FK=FK*lsb;

  //step 1 -material and B-field
  FK = 0.898*FK/(1.0-0.6*FK);
  //step 2 -low Pt
  FK=FK-26.382*FK*FK*FK*FK*FK;
  //step 3 - misalignment
  FK=FK-charge*1.408e-3;
  //Get to BMTF
  FK=FK/1.17;


  int pt=0;
  if (FK!=0)
    pt=int(2.0/FK);

  if (pt>511)
    pt=511;

  if (pt<6)
    pt=6;

  return pt;
}







L1MuKBMTrackCollection L1TMuonBarrelKalmanAlgo::clean(const L1MuKBMTrackCollection& tracks,uint seed) {
  L1MuKBMTrackCollection out;

  std::map<uint,int> infoRank;
  std::map<uint,L1MuKBMTrack> infoTrack; 
  for (uint i=3;i<=15;++i) {
    if (i==4 ||i==8)
      continue;
    infoRank[i]=-1;
  }

  for (const auto& track :tracks) {
    infoRank[track.hitPattern()] = rank(track);
    infoTrack[track.hitPattern()]=track;
  }
    

  int selected=15;
  if (seed==4) //station 4 seeded 
    {
      int sel6 = infoRank[10]>= infoRank[12] ? 10 : 12;
      int sel5 = infoRank[14]>= infoRank[9] ? 14 : 9;
      int sel4 = infoRank[11]>= infoRank[13] ? 11 : 13;
      int sel3 = infoRank[sel6]>= infoRank[sel5] ? sel6 : sel5;
      int sel2 = infoRank[sel4]>= infoRank[sel3] ? sel4 : sel3;
      selected = infoRank[15]>= infoRank[sel2] ? 15 : sel2;
    }
  if (seed==3) //station 3 seeded 
    {
      int sel2 = infoRank[5]>= infoRank[6] ? 5 : 6;
      selected = infoRank[7]>= infoRank[sel2] ? 7 : sel2;
    }
  if (seed==2) //station 3 seeded 
    selected = 3;

  auto search = infoTrack.find(selected);
  if (search != infoTrack.end())
    out.push_back(search->second);

  return out;


}





uint L1TMuonBarrelKalmanAlgo::etaStubRank(const L1MuKBMTCombinedStubRef& stub) {

  if (stub->qeta1()!=0 && stub->qeta2()!=0) {
    return 0;
  }  
  if (stub->qeta1()==0) {
    return 0;
  }
  //  return (stub->qeta1()*4+stub->stNum());
  return (stub->qeta1());

}

void L1TMuonBarrelKalmanAlgo::calculateEta(L1MuKBMTrack& track) {
  uint pattern = track.hitPattern();
  int wheel = track.stubs()[0]->whNum();
  uint awheel=fabs(wheel);
  int sign=1;
  if (wheel<0)
    sign=-1;
  uint nstubs = track.stubs().size();
  uint mask=0;
  for (unsigned int i=0;i<track.stubs().size();++i) {
    if (fabs(track.stubs()[i]->whNum())!=awheel)
      mask=mask|(1<<i);
  }
  mask=(awheel<<nstubs)|mask;
  track.setCoarseEta(sign*lutService_->coarseEta(pattern,mask));


  int sumweights=0;
  int sums=0;

  for (const auto& stub : track.stubs()) {
    uint rank = etaStubRank(stub);
    if (rank==0)
      continue;
    //    printf("Stub station=%d rank=%d values=%d %d\n",stub->stNum(),rank,stub->eta1(),stub->eta2());
    sumweights+=rank;
    sums+=rank*stub->eta1();
  }

  //0.5  0.332031 0.25 0.199219 0.164063
  float factor;
  if (sumweights==1)
    factor=1.0;
  else if (sumweights==2)
    factor = 0.5;
  else if (sumweights==3)
    factor=0.332031;
  else if (sumweights==4)
    factor=0.25;
  else if (sumweights==5)
    factor=0.199219;
  else if (sumweights==6)
    factor=0.164063;
  else
    factor=0.0;

  

  
  int eta=0;
  if (sums>0)
    eta=fp_product(factor,sums,10);
  else
    eta=-fp_product(factor,fabs(sums),10);
  

  //int eta=int(factor*sums);
  //    printf("Eta debug %f *%d=%d\n",factor,sums,eta);

  if (sumweights>0)
    track.setFineEta(eta);
}


int L1TMuonBarrelKalmanAlgo::phiAt2(const L1MuKBMTrack& track) {
  //If there is stub at station 2 use this else propagate from 1
  for (const auto& stub:track.stubs())
    if (stub->stNum()==2)
      return correctedPhi(stub,track.sector());

  int phi = track.phiAtMuon();
  int phiB = track.phiBAtMuon();


  int phiNew=phi+fp_product(phiAt2_,phiB,10);
  if (verbose_)
    printf("Phi at second station=%d\n",phiNew);
  if (phiNew>4095)
    phiNew=4095;
  if (phiNew<-4096)
    phiNew=-4096;
  return phiNew;

}