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) {
55 std::array<uint32_t, 4> & outTOB)
const {
82 Ascaled[irow][jcolumn] = Atwr[irow][jcolumn] >> 2;
83 AgBlk[irow][jcolumn] = AgBlk[irow][jcolumn] >> 2;
85 Bscaled[irow][jcolumn] = Btwr[irow][jcolumn] >> 2;
86 BgBlk[irow][jcolumn] = BgBlk[irow][jcolumn] >> 2;
88 Cscaled[irow][jcolumn] = Ctwr[irow][jcolumn] >> 2;
89 CgBlk[irow][jcolumn] = CgBlk[irow][jcolumn] >> 2;
145 int total_sumEt = 0x0;
153 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);
156 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);
159 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);
162 metTotal(A_MHT_x, A_MHT_y, B_MHT_x, B_MHT_y, C_MHT_x, C_MHT_y, MHT_x, MHT_y);
163 metTotal(A_MST_x, A_MST_y, B_MST_x, B_MST_y, C_MST_x, C_MST_y, MST_x, MST_y);
164 metTotal(A_MET_x, A_MET_y, B_MET_x, B_MET_y, C_MET_x, C_MET_y, MET_x, MET_y);
166 etTotal(A_eth, B_eth, C_eth, ETH);
167 etTotal(A_ets, B_ets, C_ets, ETS);
168 etTotal(A_etw, B_etw, C_etw, ETW);
173 int MET2 = MET_x * MET_x + MET_y * MET_y;
175 if (MET2 > 0x0FFFFFF) {
179 MET = std::sqrt(MET2);
191 std::vector<std::unique_ptr<gFEXJwoJTOB>> tobs_v;
199 outTOB[0] = (total_sumEt & 0x00000FFF) << 0;
200 outTOB[0] = outTOB[0] | (
MET & 0x00000FFF) << 12;
201 if (total_sumEt != 0) outTOB[0] = outTOB[0] | 0x00000001 << 24;
202 if (
MET != 0) outTOB[0] = outTOB[0] | 0x00000001 << 25;
203 outTOB[0] = outTOB[0] | (1 & 0x0000001F) << 26;
208 outTOB[1] = (MET_y & 0x00000FFF) << 0;
209 outTOB[1] = outTOB[1] | (MET_x & 0x00000FFF) << 12;
210 if (MET_y != 0) outTOB[1] = outTOB[1] | 0x00000001 << 24;
211 if (MET_x != 0) outTOB[1] = outTOB[1] | 0x00000001 << 25;
212 outTOB[1] = outTOB[1] | (2 & 0x0000001F) << 26;
215 outTOB[2] = (MHT_y & 0x00000FFF) << 0;
216 outTOB[2] = outTOB[2] | (MHT_x & 0x00000FFF) << 12;
217 if (MHT_y != 0) outTOB[2] = outTOB[2] | 0x00000001 << 24;
218 if (MHT_x != 0) outTOB[2] = outTOB[2] | 0x00000001 << 25;
219 outTOB[2] = outTOB[2] | (3 & 0x0000001F) << 26;
222 outTOB[3] = (MST_y & 0x00000FFF) << 0;
223 outTOB[3] = outTOB[3] | (MST_x & 0x00000FFF) << 12;
224 if (MST_y != 0) outTOB[3] = outTOB[3] | 0x00000001 << 24;
225 if (MST_x != 0) outTOB[3] = outTOB[3] | 0x00000001 << 25;
226 outTOB[3] = outTOB[3] | (4 & 0x0000001F) << 26;
229 tobs_v[0] = std::make_unique<gFEXJwoJTOB>();
230 tobs_v[0]->setWord(outTOB[0]);
231 tobs_v[0]->setQuantity1(
MET);
232 tobs_v[0]->setQuantity2(total_sumEt);
233 tobs_v[0]->setSaturation(0);
234 tobs_v[0]->setTobID(1);
235 if(
MET != 0 ) tobs_v[0]->setStatus1(1);
236 else tobs_v[0]->setStatus1(0);
237 if(total_sumEt!= 0) tobs_v[0]->setStatus2(1);
238 else tobs_v[0]->setStatus2(0);
240 tobs_v[1] = std::make_unique<gFEXJwoJTOB>();
241 tobs_v[1]->setWord(outTOB[1]);
242 tobs_v[1]->setQuantity1(MET_x);
243 tobs_v[1]->setQuantity2(MET_y);
244 tobs_v[1]->setSaturation(0);
245 tobs_v[1]->setTobID(2);
246 if( MET_x != 0 ) tobs_v[1]->setStatus1(1);
247 else tobs_v[1]->setStatus1(0);
248 if(MET_y!= 0) tobs_v[1]->setStatus2(1);
249 else tobs_v[1]->setStatus2(0);
251 tobs_v[2] = std::make_unique<gFEXJwoJTOB>();
252 tobs_v[2]->setWord(outTOB[2]);
253 tobs_v[2]->setQuantity1(MHT_x);
254 tobs_v[2]->setQuantity2(MHT_y);
255 tobs_v[2]->setSaturation(0);
256 tobs_v[2]->setTobID(3);
257 if( MHT_x != 0 ) tobs_v[2]->setStatus1(1);
258 else tobs_v[2]->setStatus1(0);
259 if(MHT_y!= 0) tobs_v[2]->setStatus2(1);
260 else tobs_v[2]->setStatus2(0);
262 tobs_v[3] = std::make_unique<gFEXJwoJTOB>();
263 tobs_v[3]->setWord(outTOB[3]);
264 tobs_v[3]->setQuantity1(MST_x);
265 tobs_v[3]->setQuantity2(MST_y);
266 tobs_v[3]->setSaturation(0);
267 tobs_v[3]->setTobID(4);
268 if( MST_x != 0 ) tobs_v[3]->setStatus1(1);
269 else tobs_v[2]->setStatus1(0);
270 if(MST_y!= 0) tobs_v[3]->setStatus2(1);
271 else tobs_v[3]->setStatus2(0);
282 int rows = twrs.size();
283 int cols = twrs[0].size();
284 for(
int irow = 0; irow <
rows; irow++ ){
285 for(
int jcolumn = 0; jcolumn<
cols; jcolumn++){
287 gBlkSum[irow][jcolumn] = 0;
288 int krowUp = (irow + 1)%32;
289 int krowDn = (irow - 1 +32)%32;
290 if( (jcolumn == 0) || (jcolumn == 6) ) {
292 gBlkSum[irow][jcolumn] =
293 twrs[irow][jcolumn] + twrs[krowUp][jcolumn] + twrs[krowDn][jcolumn] +
294 twrs[irow][jcolumn+1] + twrs[krowUp][jcolumn+1] + twrs[krowDn][jcolumn+1];
295 }
else if( (jcolumn == 5) || (jcolumn == 11)) {
297 gBlkSum[irow][jcolumn] =
298 twrs[irow][jcolumn] + twrs[krowUp][jcolumn] + twrs[krowDn][jcolumn] +
299 twrs[irow][jcolumn-1] + twrs[krowUp][jcolumn-1] + twrs[krowDn][jcolumn-1];
302 gBlkSum[irow][jcolumn] =
303 twrs[irow][jcolumn] + twrs[krowUp][jcolumn] + twrs[krowDn][jcolumn] +
304 twrs[irow][jcolumn-1] + twrs[krowUp][jcolumn-1] + twrs[krowDn][jcolumn-1] +
305 twrs[irow][jcolumn+1] + twrs[krowUp][jcolumn+1] + twrs[krowDn][jcolumn+1];
308 if( gBlkSum[irow][jcolumn] > seedThreshold) {
309 hasSeed[irow][jcolumn] = 1;
311 hasSeed[irow][jcolumn] = 0;
314 if ( gBlkSum[irow][jcolumn] < 0 )
315 gBlkSum[irow][jcolumn] = 0;
328 int aFPGA,
int bFPGA,
329 int & MHT_x,
int & MHT_y,
330 int & MST_x,
int & MST_y,
331 int & MET_x,
int & MET_y)
const {
333 gBlockthreshold = gBlockthreshold * 200 / 800;
346 for(
int jcolumn = 6; jcolumn<12; jcolumn++){
348 int frow = 2*(irow/2) + 1;
350 if(gBlkSum[irow][jcolumn] > gBlockthreshold){
351 h_tx_hi += (twrs[irow][jcolumn])*(
cosLUT(frow, 5));
352 h_ty_hi += (twrs[irow][jcolumn])*(
sinLUT(frow, 5));
354 e_tx_hi += (twrs[irow][jcolumn])*(
cosLUT(frow, 5));
355 e_ty_hi += (twrs[irow][jcolumn])*(
sinLUT(frow, 5));
360 if(gBlkSum[irow][jcolumn] > gBlockthreshold){
361 h_tx_hi += (twrs[irow][jcolumn])*(
cosLUT(irow, 5));
362 h_ty_hi += (twrs[irow][jcolumn])*(
sinLUT(irow, 5));
364 e_tx_hi += (twrs[irow][jcolumn])*(
cosLUT(irow, 5));
365 e_ty_hi += (twrs[irow][jcolumn])*(
sinLUT(irow, 5));
370 for(
int jcolumn = 0; jcolumn<6; jcolumn++){
372 int frow = 2*(irow/2) + 1;
374 if(gBlkSum[irow][jcolumn] > gBlockthreshold){
375 h_tx_lw += (twrs[irow][jcolumn])*(
cosLUT(frow, 5));
376 h_ty_lw += (twrs[irow][jcolumn])*(
sinLUT(frow, 5));
378 e_tx_lw += (twrs[irow][jcolumn])*(
cosLUT(frow, 5));
379 e_ty_lw += (twrs[irow][jcolumn])*(
sinLUT(frow, 5));
383 if(gBlkSum[irow][jcolumn] > gBlockthreshold){
384 h_tx_lw += (twrs[irow][jcolumn])*(
cosLUT(irow, 5));
385 h_ty_lw += (twrs[irow][jcolumn])*(
sinLUT(irow, 5));
387 e_tx_lw += (twrs[irow][jcolumn])*(
cosLUT(irow, 5));
388 e_ty_lw += (twrs[irow][jcolumn])*(
sinLUT(irow, 5));
399 long int fMHT_x = (h_tx_hi + h_tx_lw) ;
400 long int fMHT_y = (h_ty_hi + h_ty_lw) ;
401 long int fMST_x = (e_tx_hi + e_tx_lw) ;
402 long int fMST_y = (e_ty_hi + e_ty_lw) ;
404 MHT_x = (h_tx_hi + h_tx_lw) >> 3;
405 MHT_y = (h_ty_hi + h_ty_lw) >> 3;
406 MST_x = (e_tx_hi + e_tx_lw) >> 3;
407 MST_y = (e_ty_hi + e_ty_lw) >> 3;
415 long int fMET_x = ( aFPGA * (fMHT_x) + bFPGA * (fMST_x) ) >> 13 ;
416 long int fMET_y = ( aFPGA * (fMHT_y) + bFPGA * (fMST_y) ) >> 13 ;
424 int gBlockthreshold,
int A,
int B,
int ð,
int &ets,
int &etw)
const {
426 gBlockthreshold = gBlockthreshold * 200 / 800;
428 int64_t ethard_hi = 0;
429 int64_t etsoft_hi = 0;
430 int64_t ethard_lo = 0;
431 int64_t etsoft_lo = 0;
433 int64_t ethard = 0.0;
434 int64_t etsoft = 0.0;
436 int multiplicitiveFactor = 0;
439 multiplicitiveFactor =
cosLUT(0, 5);
441 multiplicitiveFactor =
cosLUT(1, 5);
447 for(
int jcolumn = 0; jcolumn<6; jcolumn++){
448 if(gBlkSum[irow][jcolumn] > gBlockthreshold){
449 ethard_lo = ethard_lo + twrs[irow][jcolumn]*multiplicitiveFactor;
451 etsoft_lo = etsoft_lo + twrs[irow][jcolumn]*multiplicitiveFactor;
457 for(
int jcolumn = 6; jcolumn<12; jcolumn++){
458 if(gBlkSum[irow][jcolumn] > gBlockthreshold){
459 ethard_hi = ethard_hi + twrs[irow][jcolumn]*multiplicitiveFactor;
461 etsoft_hi = etsoft_hi + twrs[irow][jcolumn]*multiplicitiveFactor;
466 ethard = ethard_hi + ethard_lo;
468 etsoft = etsoft_hi + etsoft_lo;
471 int64_t etsum_hi = ethard_hi*
A + etsoft_hi*B ;
472 if ( etsum_hi < 0 ) etsum_hi = 0;
474 int64_t etsum_lo = ethard_lo*
A + etsoft_lo*B ;
475 if ( etsum_lo < 0 ) etsum_lo = 0;
477 int64_t etsum = etsum_hi + etsum_lo;
483 etw = (etsum >>13 ) ;
486 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 B_MET_x,
int B_MET_y,
493 int C_MET_x,
int C_MET_y,
494 int & MET_x,
int & MET_y)
const {
497 MET_x = A_MET_x + B_MET_x + C_MET_x;
498 MET_y = A_MET_y + B_MET_y+ C_MET_y;
504 if (MET_x < -0x008000) MET_x = -0x08000;
505 if (MET_y < -0x008000) MET_y = -0x00800;
507 if (MET_x > 0x0007FF) MET_x = 0x0007FF;
508 if (MET_y > 0x0007FF) MET_y = 0x0007FF;
518 ET = (A_ET + B_ET + C_ET) ;
522 if( ET > 0x0000FFF) ET = 0x0000FFF;
530 float c =
static_cast<float>(phiIDX)/
std::pow(2,aw);
543 float c =
static_cast<float>(phiIDX)/
std::pow(2,aw);