24 declareInterface<IgFEXJwoJAlgo>(
this);
30 return StatusCode::SUCCESS;
36 int aFPGA_B,
int bFPGA_B,
37 int aFPGA_C,
int bFPGA_C,
38 int gXE_seedThrA,
int gXE_seedThrB,
int gXE_seedThrC) {
51 std::array<int32_t, 4> & outTOB)
const {
55 constexpr
bool SumETfast =
true;
79 Ascaled[irow][jcolumn] = Atwr[irow][jcolumn] >> 2;
80 AgBlk[irow][jcolumn] = AgBlk[irow][jcolumn] >> 2;
82 Bscaled[irow][jcolumn] = Btwr[irow][jcolumn] >> 2;
83 BgBlk[irow][jcolumn] = BgBlk[irow][jcolumn] >> 2;
85 Cscaled[irow][jcolumn] = Ctwr[irow][jcolumn] >> 2;
86 CgBlk[irow][jcolumn] = CgBlk[irow][jcolumn] >> 2;
140 int total_sumEt = 0x0;
147 metFPGA(0, Ascaled, AgBlk,
m_gBlockthresholdA,
m_aFPGA_A,
m_bFPGA_A, A_MHT_x, A_MHT_y, A_MST_x, A_MST_y, A_MET_x, A_MET_y);
151 metFPGA(1, Bscaled, BgBlk,
m_gBlockthresholdB,
m_aFPGA_B,
m_bFPGA_B, B_MHT_x, B_MHT_y, B_MST_x, B_MST_y, B_MET_x, B_MET_y);
155 metFPGA(2, Cscaled, CgBlk,
m_gBlockthresholdC,
m_aFPGA_C,
m_bFPGA_C, C_MHT_x, C_MHT_y, C_MST_x, C_MST_y, C_MET_x, C_MET_y);
159 metTotal(A_MHT_x, A_MHT_y, B_MHT_x, B_MHT_y, C_MHT_x, C_MHT_y, MHT_x, MHT_y);
160 metTotal(A_MST_x, A_MST_y, B_MST_x, B_MST_y, C_MST_x, C_MST_y, MST_x, MST_y);
161 metTotal(A_MET_x, A_MET_y, B_MET_x, B_MET_y, C_MET_x, C_MET_y, MET_x, MET_y);
163 etTotal(A_eth, B_eth, C_eth, ETH);
164 etTotal(A_ets, B_ets, C_ets, ETS);
165 etTotal(A_etw, B_etw, C_etw, ETW);
169 int MET2 = MET_x * MET_x + MET_y * MET_y;
171 if (MET2 > 0x0FFFFFF) {
175 MET = std::sqrt(MET2);
187 std::vector<std::unique_ptr<gFEXJwoJTOB>> tobs_v;
195 outTOB[0] = (total_sumEt & 0x00000FFF) << 0;
196 outTOB[0] = outTOB[0] | (
MET & 0x00000FFF) << 12;
197 if (total_sumEt != 0) outTOB[0] = outTOB[0] | 0x00000001 << 24;
198 if (
MET != 0) outTOB[0] = outTOB[0] | 0x00000001 << 25;
199 outTOB[0] = outTOB[0] | (1 & 0x0000001F) << 26;
204 outTOB[1] = (MET_y & 0x00000FFF) << 0;
205 outTOB[1] = outTOB[1] | (MET_x & 0x00000FFF) << 12;
206 if (MET_y != 0) outTOB[1] = outTOB[1] | 0x00000001 << 24;
207 if (MET_x != 0) outTOB[1] = outTOB[1] | 0x00000001 << 25;
208 outTOB[1] = outTOB[1] | (2 & 0x0000001F) << 26;
211 outTOB[2] = (MHT_y & 0x00000FFF) << 0;
212 outTOB[2] = outTOB[2] | (MHT_x & 0x00000FFF) << 12;
213 if (MHT_y != 0) outTOB[2] = outTOB[2] | 0x00000001 << 24;
214 if (MHT_x != 0) outTOB[2] = outTOB[2] | 0x00000001 << 25;
215 outTOB[2] = outTOB[2] | (3 & 0x0000001F) << 26;
218 outTOB[3] = (MST_y & 0x00000FFF) << 0;
219 outTOB[3] = outTOB[3] | (MST_x & 0x00000FFF) << 12;
220 if (MST_y != 0) outTOB[3] = outTOB[3] | 0x00000001 << 24;
221 if (MST_x != 0) outTOB[3] = outTOB[3] | 0x00000001 << 25;
222 outTOB[3] = outTOB[3] | (4 & 0x0000001F) << 26;
225 tobs_v[0] = std::make_unique<gFEXJwoJTOB>();
226 tobs_v[0]->setWord(outTOB[0]);
227 tobs_v[0]->setQuantity1(
MET);
228 tobs_v[0]->setQuantity2(total_sumEt);
229 tobs_v[0]->setSaturation(0);
230 tobs_v[0]->setTobID(1);
231 if(
MET != 0 ) tobs_v[0]->setStatus1(1);
232 else tobs_v[0]->setStatus1(0);
233 if(total_sumEt!= 0) tobs_v[0]->setStatus2(1);
234 else tobs_v[0]->setStatus2(0);
236 tobs_v[1] = std::make_unique<gFEXJwoJTOB>();
237 tobs_v[1]->setWord(outTOB[1]);
238 tobs_v[1]->setQuantity1(MET_x);
239 tobs_v[1]->setQuantity2(MET_y);
240 tobs_v[1]->setSaturation(0);
241 tobs_v[1]->setTobID(2);
242 if( MET_x != 0 ) tobs_v[1]->setStatus1(1);
243 else tobs_v[1]->setStatus1(0);
244 if(MET_y!= 0) tobs_v[1]->setStatus2(1);
245 else tobs_v[1]->setStatus2(0);
247 tobs_v[2] = std::make_unique<gFEXJwoJTOB>();
248 tobs_v[2]->setWord(outTOB[2]);
249 tobs_v[2]->setQuantity1(MHT_x);
250 tobs_v[2]->setQuantity2(MHT_y);
251 tobs_v[2]->setSaturation(0);
252 tobs_v[2]->setTobID(3);
253 if( MHT_x != 0 ) tobs_v[2]->setStatus1(1);
254 else tobs_v[2]->setStatus1(0);
255 if(MHT_y!= 0) tobs_v[2]->setStatus2(1);
256 else tobs_v[2]->setStatus2(0);
258 tobs_v[3] = std::make_unique<gFEXJwoJTOB>();
259 tobs_v[3]->setWord(outTOB[3]);
260 tobs_v[3]->setQuantity1(MST_x);
261 tobs_v[3]->setQuantity2(MST_y);
262 tobs_v[3]->setSaturation(0);
263 tobs_v[3]->setTobID(4);
264 if( MST_x != 0 ) tobs_v[3]->setStatus1(1);
265 else tobs_v[2]->setStatus1(0);
266 if(MST_y!= 0) tobs_v[3]->setStatus2(1);
267 else tobs_v[3]->setStatus2(0);
278 int rows = twrs.size();
279 int cols = twrs[0].size();
280 for(
int irow = 0; irow <
rows; irow++ ){
281 for(
int jcolumn = 0; jcolumn<
cols; jcolumn++){
283 gBlkSum[irow][jcolumn] = 0;
284 int krowUp = (irow + 1)%32;
285 int krowDn = (irow - 1 +32)%32;
286 if( (jcolumn == 0) || (jcolumn == 6) ) {
288 gBlkSum[irow][jcolumn] =
289 twrs[irow][jcolumn] + twrs[krowUp][jcolumn] + twrs[krowDn][jcolumn] +
290 twrs[irow][jcolumn+1] + twrs[krowUp][jcolumn+1] + twrs[krowDn][jcolumn+1];
291 }
else if( (jcolumn == 5) || (jcolumn == 11)) {
293 gBlkSum[irow][jcolumn] =
294 twrs[irow][jcolumn] + twrs[krowUp][jcolumn] + twrs[krowDn][jcolumn] +
295 twrs[irow][jcolumn-1] + twrs[krowUp][jcolumn-1] + twrs[krowDn][jcolumn-1];
298 gBlkSum[irow][jcolumn] =
299 twrs[irow][jcolumn] + twrs[krowUp][jcolumn] + twrs[krowDn][jcolumn] +
300 twrs[irow][jcolumn-1] + twrs[krowUp][jcolumn-1] + twrs[krowDn][jcolumn-1] +
301 twrs[irow][jcolumn+1] + twrs[krowUp][jcolumn+1] + twrs[krowDn][jcolumn+1];
304 if( gBlkSum[irow][jcolumn] > seedThreshold) {
305 hasSeed[irow][jcolumn] = 1;
307 hasSeed[irow][jcolumn] = 0;
310 if ( gBlkSum[irow][jcolumn] < 0 )
311 gBlkSum[irow][jcolumn] = 0;
324 int aFPGA,
int bFPGA,
325 int & MHT_x,
int & MHT_y,
326 int & MST_x,
int & MST_y,
327 int & MET_x,
int & MET_y)
const {
329 gBlockthreshold = gBlockthreshold * 200 / 800;
342 for(
int jcolumn = 6; jcolumn<12; jcolumn++){
344 int frow = 2*(irow/2) + 1;
346 if(gBlkSum[irow][jcolumn] > gBlockthreshold){
347 h_tx_hi += (twrs[irow][jcolumn])*(
cosLUT(frow, 5));
348 h_ty_hi += (twrs[irow][jcolumn])*(
sinLUT(frow, 5));
350 e_tx_hi += (twrs[irow][jcolumn])*(
cosLUT(frow, 5));
351 e_ty_hi += (twrs[irow][jcolumn])*(
sinLUT(frow, 5));
356 if(gBlkSum[irow][jcolumn] > gBlockthreshold){
357 h_tx_hi += (twrs[irow][jcolumn])*(
cosLUT(irow, 5));
358 h_ty_hi += (twrs[irow][jcolumn])*(
sinLUT(irow, 5));
360 e_tx_hi += (twrs[irow][jcolumn])*(
cosLUT(irow, 5));
361 e_ty_hi += (twrs[irow][jcolumn])*(
sinLUT(irow, 5));
366 for(
int jcolumn = 0; jcolumn<6; jcolumn++){
368 int frow = 2*(irow/2) + 1;
370 if(gBlkSum[irow][jcolumn] > gBlockthreshold){
371 h_tx_lw += (twrs[irow][jcolumn])*(
cosLUT(frow, 5));
372 h_ty_lw += (twrs[irow][jcolumn])*(
sinLUT(frow, 5));
374 e_tx_lw += (twrs[irow][jcolumn])*(
cosLUT(frow, 5));
375 e_ty_lw += (twrs[irow][jcolumn])*(
sinLUT(frow, 5));
379 if(gBlkSum[irow][jcolumn] > gBlockthreshold){
380 h_tx_lw += (twrs[irow][jcolumn])*(
cosLUT(irow, 5));
381 h_ty_lw += (twrs[irow][jcolumn])*(
sinLUT(irow, 5));
383 e_tx_lw += (twrs[irow][jcolumn])*(
cosLUT(irow, 5));
384 e_ty_lw += (twrs[irow][jcolumn])*(
sinLUT(irow, 5));
395 long int fMHT_x = (h_tx_hi + h_tx_lw) ;
396 long int fMHT_y = (h_ty_hi + h_ty_lw) ;
397 long int fMST_x = (e_tx_hi + e_tx_lw) ;
398 long int fMST_y = (e_ty_hi + e_ty_lw) ;
400 MHT_x = (h_tx_hi + h_tx_lw) >> 3;
401 MHT_y = (h_ty_hi + h_ty_lw) >> 3;
402 MST_x = (e_tx_hi + e_tx_lw) >> 3;
403 MST_y = (e_ty_hi + e_ty_lw) >> 3;
411 long int fMET_x = ( aFPGA * (fMHT_x) + bFPGA * (fMST_x) ) >> 13 ;
412 long int fMET_y = ( aFPGA * (fMHT_y) + bFPGA * (fMST_y) ) >> 13 ;
420 int gBlockthreshold,
int A,
int B,
int ð,
int &ets,
int &etw)
const {
422 gBlockthreshold = gBlockthreshold * 200 / 800;
424 int64_t ethard_hi = 0;
425 int64_t etsoft_hi = 0;
426 int64_t ethard_lo = 0;
427 int64_t etsoft_lo = 0;
429 int64_t ethard = 0.0;
430 int64_t etsoft = 0.0;
432 int multiplicitiveFactor = 0;
435 multiplicitiveFactor =
cosLUT(0, 5);
437 multiplicitiveFactor =
cosLUT(1, 5);
443 for(
int jcolumn = 0; jcolumn<6; jcolumn++){
444 if(gBlkSum[irow][jcolumn] > gBlockthreshold){
445 ethard_lo = ethard_lo + twrs[irow][jcolumn]*multiplicitiveFactor;
447 etsoft_lo = etsoft_lo + twrs[irow][jcolumn]*multiplicitiveFactor;
453 for(
int jcolumn = 6; jcolumn<12; jcolumn++){
454 if(gBlkSum[irow][jcolumn] > gBlockthreshold){
455 ethard_hi = ethard_hi + twrs[irow][jcolumn]*multiplicitiveFactor;
457 etsoft_hi = etsoft_hi + twrs[irow][jcolumn]*multiplicitiveFactor;
462 ethard = ethard_hi + ethard_lo;
464 etsoft = etsoft_hi + etsoft_lo;
467 int64_t etsum_hi = ethard_hi*
A + etsoft_hi*B ;
468 if ( etsum_hi < 0 ) etsum_hi = 0;
470 int64_t etsum_lo = ethard_lo*
A + etsoft_lo*B ;
471 if ( etsum_lo < 0 ) etsum_lo = 0;
473 int64_t etsum = etsum_hi + etsum_lo;
479 etw = (etsum >>13 ) ;
481 if( etw < 0 ) etw = 0;
483 if( etw > 0X001FFFF ) etw = 0X001FFFF ;
487 std::cout <<
"DMS FPGA gTEJWOJ " << std::hex << FPGAnum <<
"et sum hard " << eth <<
"etsum soft" << ets <<
" A " <<
A <<
" B " << B <<
" weighted term " << etw << std::endl << std::dec;
492 int gBlockthreshold,
int A,
int B,
int ð,
int &ets,
int &etw)
const {
494 gBlockthreshold = gBlockthreshold * 200 / 800;
495 int64_t ethard_hi = 0;
496 int64_t etsoft_hi = 0;
497 int64_t ethard_lo = 0;
498 int64_t etsoft_lo = 0;
500 int64_t ethard = 0.0;
501 int64_t etsoft = 0.0;
505 for(
int jcolumn = 0; jcolumn<6; jcolumn++){
506 if(gBlkSum[irow][jcolumn] > gBlockthreshold){
507 ethard_lo = ethard_lo + twrs[irow][jcolumn];
510 etsoft_lo = etsoft_lo + twrs[irow][jcolumn];
516 for(
int jcolumn = 6; jcolumn<12; jcolumn++){
517 if(gBlkSum[irow][jcolumn] > gBlockthreshold){
518 ethard_hi = ethard_hi + twrs[irow][jcolumn];
521 etsoft_hi = etsoft_hi + twrs[irow][jcolumn];
526 ethard = ethard_hi + ethard_lo;
527 etsoft = etsoft_hi + etsoft_lo;
535 if( etw < -32768 ) etw = -32768;
536 if( etw > 32767 ) etw = 32767;
539 std::cout <<
"DMS FPGA gTEJWOJ " << std::hex << FPGAnum <<
"et sum hard " << eth <<
"etsum soft" << ets <<
" A " <<
A <<
" B " << B <<
" weighted term " << etw << std::endl << std::dec;
544 int B_MET_x,
int B_MET_y,
545 int C_MET_x,
int C_MET_y,
546 int & MET_x,
int & MET_y)
const {
549 MET_x = A_MET_x + B_MET_x + C_MET_x;
550 MET_y = A_MET_y + B_MET_y+ C_MET_y;
556 if (MET_x < -0x000800) MET_x = -0x000800;
557 if (MET_y < -0x000800) MET_y = -0x000800;
559 if (MET_x > 0x0007FF) MET_x = 0x0007FF;
560 if (MET_y > 0x0007FF) MET_y = 0x0007FF;
570 if (A_ET < 0 ) A_ET = 0;
571 if (B_ET < 0 ) B_ET = 0;
572 if (C_ET < 0 ) C_ET = 0;
574 ET = (A_ET + B_ET + C_ET) ;
577 if( ET > 0x0000FFF) ET = 0x0000FFF;
586 float c =
static_cast<float>(phiIDX)/
std::pow(2,aw);
599 float c =
static_cast<float>(phiIDX)/
std::pow(2,aw);