25 declareInterface<IgFEXJwoJAlgo>(
this);
31 return StatusCode::SUCCESS;
37 int aFPGA_B,
int bFPGA_B,
38 int aFPGA_C,
int bFPGA_C,
39 int gXE_seedThrA,
int gXE_seedThrB,
int gXE_seedThrC) {
52 std::array<int32_t, 4> & outTOB)
const {
56 bool SumETfast =
true;
80 Ascaled[irow][jcolumn] = Atwr[irow][jcolumn] >> 2;
81 AgBlk[irow][jcolumn] = AgBlk[irow][jcolumn] >> 2;
83 Bscaled[irow][jcolumn] = Btwr[irow][jcolumn] >> 2;
84 BgBlk[irow][jcolumn] = BgBlk[irow][jcolumn] >> 2;
86 Cscaled[irow][jcolumn] = Ctwr[irow][jcolumn] >> 2;
87 CgBlk[irow][jcolumn] = CgBlk[irow][jcolumn] >> 2;
141 int total_sumEt = 0x0;
148 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);
152 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);
156 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);
160 metTotal(A_MHT_x, A_MHT_y, B_MHT_x, B_MHT_y, C_MHT_x, C_MHT_y, MHT_x, MHT_y);
161 metTotal(A_MST_x, A_MST_y, B_MST_x, B_MST_y, C_MST_x, C_MST_y, MST_x, MST_y);
162 metTotal(A_MET_x, A_MET_y, B_MET_x, B_MET_y, C_MET_x, C_MET_y, MET_x, MET_y);
164 etTotal(A_eth, B_eth, C_eth, ETH);
165 etTotal(A_ets, B_ets, C_ets, ETS);
166 etTotal(A_etw, B_etw, C_etw, ETW);
170 int MET2 = MET_x * MET_x + MET_y * MET_y;
172 if (MET2 > 0x0FFFFFF) {
176 MET = std::sqrt(MET2);
188 std::vector<std::unique_ptr<gFEXJwoJTOB>> tobs_v;
196 outTOB[0] = (total_sumEt & 0x00000FFF) << 0;
197 outTOB[0] = outTOB[0] | (
MET & 0x00000FFF) << 12;
198 if (total_sumEt != 0) outTOB[0] = outTOB[0] | 0x00000001 << 24;
199 if (
MET != 0) outTOB[0] = outTOB[0] | 0x00000001 << 25;
200 outTOB[0] = outTOB[0] | (1 & 0x0000001F) << 26;
205 outTOB[1] = (MET_y & 0x00000FFF) << 0;
206 outTOB[1] = outTOB[1] | (MET_x & 0x00000FFF) << 12;
207 if (MET_y != 0) outTOB[1] = outTOB[1] | 0x00000001 << 24;
208 if (MET_x != 0) outTOB[1] = outTOB[1] | 0x00000001 << 25;
209 outTOB[1] = outTOB[1] | (2 & 0x0000001F) << 26;
212 outTOB[2] = (MHT_y & 0x00000FFF) << 0;
213 outTOB[2] = outTOB[2] | (MHT_x & 0x00000FFF) << 12;
214 if (MHT_y != 0) outTOB[2] = outTOB[2] | 0x00000001 << 24;
215 if (MHT_x != 0) outTOB[2] = outTOB[2] | 0x00000001 << 25;
216 outTOB[2] = outTOB[2] | (3 & 0x0000001F) << 26;
219 outTOB[3] = (MST_y & 0x00000FFF) << 0;
220 outTOB[3] = outTOB[3] | (MST_x & 0x00000FFF) << 12;
221 if (MST_y != 0) outTOB[3] = outTOB[3] | 0x00000001 << 24;
222 if (MST_x != 0) outTOB[3] = outTOB[3] | 0x00000001 << 25;
223 outTOB[3] = outTOB[3] | (4 & 0x0000001F) << 26;
226 tobs_v[0] = std::make_unique<gFEXJwoJTOB>();
227 tobs_v[0]->setWord(outTOB[0]);
228 tobs_v[0]->setQuantity1(
MET);
229 tobs_v[0]->setQuantity2(total_sumEt);
230 tobs_v[0]->setSaturation(0);
231 tobs_v[0]->setTobID(1);
232 if(
MET != 0 ) tobs_v[0]->setStatus1(1);
233 else tobs_v[0]->setStatus1(0);
234 if(total_sumEt!= 0) tobs_v[0]->setStatus2(1);
235 else tobs_v[0]->setStatus2(0);
237 tobs_v[1] = std::make_unique<gFEXJwoJTOB>();
238 tobs_v[1]->setWord(outTOB[1]);
239 tobs_v[1]->setQuantity1(MET_x);
240 tobs_v[1]->setQuantity2(MET_y);
241 tobs_v[1]->setSaturation(0);
242 tobs_v[1]->setTobID(2);
243 if( MET_x != 0 ) tobs_v[1]->setStatus1(1);
244 else tobs_v[1]->setStatus1(0);
245 if(MET_y!= 0) tobs_v[1]->setStatus2(1);
246 else tobs_v[1]->setStatus2(0);
248 tobs_v[2] = std::make_unique<gFEXJwoJTOB>();
249 tobs_v[2]->setWord(outTOB[2]);
250 tobs_v[2]->setQuantity1(MHT_x);
251 tobs_v[2]->setQuantity2(MHT_y);
252 tobs_v[2]->setSaturation(0);
253 tobs_v[2]->setTobID(3);
254 if( MHT_x != 0 ) tobs_v[2]->setStatus1(1);
255 else tobs_v[2]->setStatus1(0);
256 if(MHT_y!= 0) tobs_v[2]->setStatus2(1);
257 else tobs_v[2]->setStatus2(0);
259 tobs_v[3] = std::make_unique<gFEXJwoJTOB>();
260 tobs_v[3]->setWord(outTOB[3]);
261 tobs_v[3]->setQuantity1(MST_x);
262 tobs_v[3]->setQuantity2(MST_y);
263 tobs_v[3]->setSaturation(0);
264 tobs_v[3]->setTobID(4);
265 if( MST_x != 0 ) tobs_v[3]->setStatus1(1);
266 else tobs_v[2]->setStatus1(0);
267 if(MST_y!= 0) tobs_v[3]->setStatus2(1);
268 else tobs_v[3]->setStatus2(0);
279 int rows = twrs.size();
280 int cols = twrs[0].size();
281 for(
int irow = 0; irow <
rows; irow++ ){
282 for(
int jcolumn = 0; jcolumn<
cols; jcolumn++){
284 gBlkSum[irow][jcolumn] = 0;
285 int krowUp = (irow + 1)%32;
286 int krowDn = (irow - 1 +32)%32;
287 if( (jcolumn == 0) || (jcolumn == 6) ) {
289 gBlkSum[irow][jcolumn] =
290 twrs[irow][jcolumn] + twrs[krowUp][jcolumn] + twrs[krowDn][jcolumn] +
291 twrs[irow][jcolumn+1] + twrs[krowUp][jcolumn+1] + twrs[krowDn][jcolumn+1];
292 }
else if( (jcolumn == 5) || (jcolumn == 11)) {
294 gBlkSum[irow][jcolumn] =
295 twrs[irow][jcolumn] + twrs[krowUp][jcolumn] + twrs[krowDn][jcolumn] +
296 twrs[irow][jcolumn-1] + twrs[krowUp][jcolumn-1] + twrs[krowDn][jcolumn-1];
299 gBlkSum[irow][jcolumn] =
300 twrs[irow][jcolumn] + twrs[krowUp][jcolumn] + twrs[krowDn][jcolumn] +
301 twrs[irow][jcolumn-1] + twrs[krowUp][jcolumn-1] + twrs[krowDn][jcolumn-1] +
302 twrs[irow][jcolumn+1] + twrs[krowUp][jcolumn+1] + twrs[krowDn][jcolumn+1];
305 if( gBlkSum[irow][jcolumn] > seedThreshold) {
306 hasSeed[irow][jcolumn] = 1;
308 hasSeed[irow][jcolumn] = 0;
311 if ( gBlkSum[irow][jcolumn] < 0 )
312 gBlkSum[irow][jcolumn] = 0;
325 int aFPGA,
int bFPGA,
326 int & MHT_x,
int & MHT_y,
327 int & MST_x,
int & MST_y,
328 int & MET_x,
int & MET_y)
const {
330 gBlockthreshold = gBlockthreshold * 200 / 800;
343 for(
int jcolumn = 6; jcolumn<12; jcolumn++){
345 int frow = 2*(irow/2) + 1;
347 if(gBlkSum[irow][jcolumn] > gBlockthreshold){
348 h_tx_hi += (twrs[irow][jcolumn])*(
cosLUT(frow, 5));
349 h_ty_hi += (twrs[irow][jcolumn])*(
sinLUT(frow, 5));
351 e_tx_hi += (twrs[irow][jcolumn])*(
cosLUT(frow, 5));
352 e_ty_hi += (twrs[irow][jcolumn])*(
sinLUT(frow, 5));
357 if(gBlkSum[irow][jcolumn] > gBlockthreshold){
358 h_tx_hi += (twrs[irow][jcolumn])*(
cosLUT(irow, 5));
359 h_ty_hi += (twrs[irow][jcolumn])*(
sinLUT(irow, 5));
361 e_tx_hi += (twrs[irow][jcolumn])*(
cosLUT(irow, 5));
362 e_ty_hi += (twrs[irow][jcolumn])*(
sinLUT(irow, 5));
367 for(
int jcolumn = 0; jcolumn<6; jcolumn++){
369 int frow = 2*(irow/2) + 1;
371 if(gBlkSum[irow][jcolumn] > gBlockthreshold){
372 h_tx_lw += (twrs[irow][jcolumn])*(
cosLUT(frow, 5));
373 h_ty_lw += (twrs[irow][jcolumn])*(
sinLUT(frow, 5));
375 e_tx_lw += (twrs[irow][jcolumn])*(
cosLUT(frow, 5));
376 e_ty_lw += (twrs[irow][jcolumn])*(
sinLUT(frow, 5));
380 if(gBlkSum[irow][jcolumn] > gBlockthreshold){
381 h_tx_lw += (twrs[irow][jcolumn])*(
cosLUT(irow, 5));
382 h_ty_lw += (twrs[irow][jcolumn])*(
sinLUT(irow, 5));
384 e_tx_lw += (twrs[irow][jcolumn])*(
cosLUT(irow, 5));
385 e_ty_lw += (twrs[irow][jcolumn])*(
sinLUT(irow, 5));
396 long int fMHT_x = (h_tx_hi + h_tx_lw) ;
397 long int fMHT_y = (h_ty_hi + h_ty_lw) ;
398 long int fMST_x = (e_tx_hi + e_tx_lw) ;
399 long int fMST_y = (e_ty_hi + e_ty_lw) ;
401 MHT_x = (h_tx_hi + h_tx_lw) >> 3;
402 MHT_y = (h_ty_hi + h_ty_lw) >> 3;
403 MST_x = (e_tx_hi + e_tx_lw) >> 3;
404 MST_y = (e_ty_hi + e_ty_lw) >> 3;
412 long int fMET_x = ( aFPGA * (fMHT_x) + bFPGA * (fMST_x) ) >> 13 ;
413 long int fMET_y = ( aFPGA * (fMHT_y) + bFPGA * (fMST_y) ) >> 13 ;
421 int gBlockthreshold,
int A,
int B,
int ð,
int &ets,
int &etw)
const {
423 gBlockthreshold = gBlockthreshold * 200 / 800;
425 int64_t ethard_hi = 0;
426 int64_t etsoft_hi = 0;
427 int64_t ethard_lo = 0;
428 int64_t etsoft_lo = 0;
430 int64_t ethard = 0.0;
431 int64_t etsoft = 0.0;
433 int multiplicitiveFactor = 0;
436 multiplicitiveFactor =
cosLUT(0, 5);
438 multiplicitiveFactor =
cosLUT(1, 5);
444 for(
int jcolumn = 0; jcolumn<6; jcolumn++){
445 if(gBlkSum[irow][jcolumn] > gBlockthreshold){
446 ethard_lo = ethard_lo + twrs[irow][jcolumn]*multiplicitiveFactor;
448 etsoft_lo = etsoft_lo + twrs[irow][jcolumn]*multiplicitiveFactor;
454 for(
int jcolumn = 6; jcolumn<12; jcolumn++){
455 if(gBlkSum[irow][jcolumn] > gBlockthreshold){
456 ethard_hi = ethard_hi + twrs[irow][jcolumn]*multiplicitiveFactor;
458 etsoft_hi = etsoft_hi + twrs[irow][jcolumn]*multiplicitiveFactor;
463 ethard = ethard_hi + ethard_lo;
465 etsoft = etsoft_hi + etsoft_lo;
468 int64_t etsum_hi = ethard_hi*
A + etsoft_hi*B ;
469 if ( etsum_hi < 0 ) etsum_hi = 0;
471 int64_t etsum_lo = ethard_lo*
A + etsoft_lo*B ;
472 if ( etsum_lo < 0 ) etsum_lo = 0;
474 int64_t etsum = etsum_hi + etsum_lo;
480 etw = (etsum >>13 ) ;
482 if( etw < 0 ) etw = 0;
484 if( etw > 0X001FFFF ) etw = 0X001FFFF ;
488 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;
493 int gBlockthreshold,
int A,
int B,
int ð,
int &ets,
int &etw)
const {
495 gBlockthreshold = gBlockthreshold * 200 / 800;
496 int64_t ethard_hi = 0;
497 int64_t etsoft_hi = 0;
498 int64_t ethard_lo = 0;
499 int64_t etsoft_lo = 0;
501 int64_t ethard = 0.0;
502 int64_t etsoft = 0.0;
506 for(
int jcolumn = 0; jcolumn<6; jcolumn++){
507 if(gBlkSum[irow][jcolumn] > gBlockthreshold){
508 ethard_lo = ethard_lo + twrs[irow][jcolumn];
511 etsoft_lo = etsoft_lo + twrs[irow][jcolumn];
517 for(
int jcolumn = 6; jcolumn<12; jcolumn++){
518 if(gBlkSum[irow][jcolumn] > gBlockthreshold){
519 ethard_hi = ethard_hi + twrs[irow][jcolumn];
522 etsoft_hi = etsoft_hi + twrs[irow][jcolumn];
527 ethard = ethard_hi + ethard_lo;
528 etsoft = etsoft_hi + etsoft_lo;
536 if( etw < -32768 ) etw = -32768;
537 if( etw > 32767 ) etw = 32767;
540 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;
545 int B_MET_x,
int B_MET_y,
546 int C_MET_x,
int C_MET_y,
547 int & MET_x,
int & MET_y)
const {
550 MET_x = A_MET_x + B_MET_x + C_MET_x;
551 MET_y = A_MET_y + B_MET_y+ C_MET_y;
557 if (MET_x < -0x000800) MET_x = -0x000800;
558 if (MET_y < -0x000800) MET_y = -0x000800;
560 if (MET_x > 0x0007FF) MET_x = 0x0007FF;
561 if (MET_y > 0x0007FF) MET_y = 0x0007FF;
571 if (A_ET < 0 ) A_ET = 0;
572 if (B_ET < 0 ) B_ET = 0;
573 if (C_ET < 0 ) C_ET = 0;
575 ET = (A_ET + B_ET + C_ET) ;
578 if( ET > 0x0000FFF) ET = 0x0000FFF;
587 float c =
static_cast<float>(phiIDX)/
std::pow(2,aw);
600 float c =
static_cast<float>(phiIDX)/
std::pow(2,aw);