Belle II Software  release-05-01-25
Fitter3DUtility.cc
1 #ifndef __EXTERNAL__
2 #include "trg/cdc/Fitter3DUtility.h"
3 #include "trg/cdc/FpgaUtility.h"
4 #include "trg/cdc/JLUT.h"
5 #include "trg/cdc/JSignal.h"
6 #include "trg/cdc/JSignalData.h"
7 #include <cmath>
8 #else
9 #include "Fitter3DUtility.h"
10 #include "JLUT.h"
11 #include "JSignal.h"
12 #include "JSignalData.h"
13 #endif
14 #include <utility>
15 #include <iostream>
16 #include <fstream>
17 #include "TLorentzVector.h"
18 #include "TVector3.h"
19 #include "TVector2.h"
20 
21 using std::cout;
22 using std::endl;
23 using std::pair;
24 using std::make_pair;
25 using std::tuple;
26 using std::vector;
27 using std::map;
28 using std::string;
29 using std::to_string;
30 using std::get;
31 using std::function;
32 using std::ofstream;
33 using std::ifstream;
34 using std::make_tuple;
35 
36 int Fitter3DUtility::findSign(double* phi2)
37 {
38  double Trg_PI = 3.141592653589793;
39  int mysign;
40  double sign_phi[2];
41 
42  if ((phi2[0] - phi2[4]) > Trg_PI || (phi2[0] - phi2[4]) < -Trg_PI) {
43  if (phi2[0] > Trg_PI) {sign_phi[0] = phi2[0] - 2 * Trg_PI;}
44  else {sign_phi[0] = phi2[0];}
45  if (phi2[4] > Trg_PI) {sign_phi[1] = phi2[4] - 2 * Trg_PI;}
46  else {sign_phi[1] = phi2[4];}
47  } else {
48  sign_phi[0] = phi2[0];
49  sign_phi[1] = phi2[4];
50  }
51  if ((sign_phi[1] - sign_phi[0]) > 0) {mysign = 0;}
52  else {mysign = 1;}
53 
54  return mysign;
55 }
56 
57 void Fitter3DUtility::rPhiFit(double* rr, double* phi2, double* phierror, double& rho, double& myphi0)
58 {
59 
60  cout << "Fitter3DUtility::rPhiFit() will be deprecated. Please use Fitter3DUtility::rPhiFitter(). phierror was changed to inv phi error."
61  << endl;
62  double invphierror[5];
63  for (unsigned i = 0; i < 5; i++) {
64  invphierror[i] = 1 / phierror[i];
65  }
66  rPhiFitter(rr, phi2, invphierror, rho, myphi0);
67 }
68 
69 void Fitter3DUtility::rPhiFitter(double* rr, double* phi2, double* invphierror, double& rho, double& myphi0)
70 {
71 
72  double chi2;
73  rPhiFitter(rr, phi2, invphierror, rho, myphi0, chi2);
74 
75 }
76 
77 void Fitter3DUtility::rPhiFitter(double* rr, double* phi2, double* invphierror, double& rho, double& myphi0, double& chi2)
78 {
79 
80  // Print input values
81  //for(unsigned iSL=0; iSL<5; iSL++){
82  // cout<<"SL["<<iSL<<"] rr: "<<rr[iSL]<<" phi: "<<phi2[iSL]<<" phiError: "<<phierror[iSL]<<endl;
83  //}
84 
85  double Trg_PI = 3.141592653589793;
86  double A, B, C, D, E, hcx, hcy;
87  double invFiterror[5];
88  //double G;
89  //Calculate fit error
90  for (unsigned i = 0; i < 5; i++) {
91  // Sometimes SL8 and SL4 will not be hit. So cannot use below calculation.
92  //invFiterror[i]=1/sqrt((rr[4]*rr[4]-2*rr[4]*rr[2]*cos(phi2[4]-phi2[2])+rr[2]*rr[2])/(sin(phi2[4]-phi2[2])*sin(phi2[4]-phi2[2]))-rr[i]*rr[i])*invphierror[i];
93  //invFiterror[i]=invphierror[i];
94  //invFiterror[i]=invphierror[i]*rr[i];
95  invFiterror[i] = invphierror[i] / rr[i];
96  }
97 
98  //r-phi fitter(2D Fitter) ->calculate pt and radius of track-> input for 3D fitter.
99  A = 0, B = 0, C = 0, D = 0, E = 0, hcx = 0, hcy = 0;
100  //G=0;
101  for (unsigned i = 0; i < 5; i++) {
102  A += cos(phi2[i]) * cos(phi2[i]) * (invFiterror[i] * invFiterror[i]);
103  B += sin(phi2[i]) * sin(phi2[i]) * (invFiterror[i] * invFiterror[i]);
104  C += cos(phi2[i]) * sin(phi2[i]) * (invFiterror[i] * invFiterror[i]);
105  D += rr[i] * cos(phi2[i]) * (invFiterror[i] * invFiterror[i]);
106  E += rr[i] * sin(phi2[i]) * (invFiterror[i] * invFiterror[i]);
107  //G+=rr[i]*rr[i]/(fiterror[i]*fiterror[i]);
108  }
109  hcx = D * B - E * C; //helix center x
110  hcx /= 2 * (A * B - C * C);
111  hcy = E * A - D * C; //helix center y
112  hcy /= 2 * (A * B - C * C);
113  rho = sqrt(hcx * hcx + hcy * hcy); //radius of helix
114  myphi0 = atan2(hcy, hcx);
115  if (myphi0 < 0) myphi0 += 2 * Trg_PI;
116  //myphi0=atan(hcy/hcx);
117  //if(hcx<0 && hcy>0) myphi0 += Trg_PI;
118  //if(hcx<0 && hcy<0) myphi0 += Trg_PI;
119  //if(hcx>0 && hcy<0) myphi0 += Trg_PI*2.0;
120 
122  // double pchi2 = -2*hcx*D-2*hcy*E+G;
123  // pchi2/=3;
124 
125  // Count number of total TS hits.
126  int nTSHits = 0;
127  for (unsigned iAx = 0; iAx < 5; iAx++) {
128  if (invphierror[iAx] != 0) nTSHits++;
129  }
130  // Another way to calculate chi2
131  chi2 = 0.;
132  for (unsigned i = 0; i < 5; i++) {
133  chi2 += (2 * (hcx * cos(phi2[i]) + hcy * sin(phi2[i])) - rr[i]) * (2 * (hcx * cos(phi2[i]) + hcy * sin(phi2[i])) - rr[i]) /
134  (invFiterror[i] * invFiterror[i]);
135  }
136  chi2 /= nTSHits - 2;
137 
138 }
139 
140 void Fitter3DUtility::chargeFinder(double* nTSs, double* tsIds, double* tsHitmap, double phi0, double inCharge,
141  double& foundCharge)
142 {
143  double Trg_PI = 3.141592653589793;
144  vector<double> tsPhi(5);
145  vector<double> dPhi(5);
146  vector<double> dPhi_c(5);
147  vector<double> charge(5);
148  for (unsigned iAx = 0; iAx < 5; iAx++) {
149  //cout<<"iAx:"<<iAx<<" nTSs:"<<nTSs[iAx]<<" tsId:"<<tsIds[iAx]<<" hitmap:"<<tsHitmap[iAx]<<" phi0:"<<phi0<<" inCharge:"<<inCharge<<endl;
150  // Change tsId to rad unit.
151  tsPhi[iAx] = tsIds[iAx] * 2 * Trg_PI / nTSs[iAx];
152  dPhi[iAx] = tsPhi[iAx] - phi0;
153  //cout<<"iAx:"<<iAx<<" dPhi:"<<dPhi[iAx]<<endl;
154  // Constrain dPhi to [-pi, pi]
155  if (dPhi[iAx] < -Trg_PI) dPhi_c[iAx] = dPhi[iAx] + 2 * Trg_PI;
156  else if (dPhi[iAx] > Trg_PI) dPhi_c[iAx] = dPhi[iAx] - 2 * Trg_PI;
157  else dPhi_c[iAx] = dPhi[iAx];
158  //cout<<"iAx:"<<iAx<<" dPhi_c:"<<dPhi_c[iAx]<<endl;
159  // Calculate charge
160  if (tsHitmap[iAx] == 0) charge[iAx] = 0;
161  else if (dPhi_c[iAx] == 0) charge[iAx] = 0;
162  else if (dPhi_c[iAx] > 0) charge[iAx] = 1;
163  else charge[iAx] = -1;
164  //cout<<"iAx:"<<iAx<<" charge:"<<charge[iAx]<<endl;
165  }
166  // Sum up charge
167  double sumCharge = charge[0] + charge[1] + charge[2] + charge[3] + charge[4];
168  //cout<<"sumCharge:"<<sumCharge<<endl;
169  // Calculate foundCharge
170  if (sumCharge == 0) foundCharge = inCharge;
171  else if (sumCharge > 0) foundCharge = 1;
172  else foundCharge = -1;
173  //cout<<"foundCharge:"<<foundCharge<<endl;
174  //cout<<"inCharge:"<<inCharge<<endl;
175 }
176 
177 
178 void Fitter3DUtility::rPhiFit2(double* rr, double* phi2, double* phierror, double& rho, double& myphi0, int nTS)
179 {
180 
181  double Trg_PI = 3.141592653589793;
182  double A, B, C, D, E, hcx, hcy;
183  //double G;
184  double fiterror[5];
185  //Calculate fit error
186  for (int i = 0; i < nTS; i++) {
187  //fiterror[i]=sqrt((rr[4]*rr[4]-2*rr[4]*rr[2]*cos(phi2[4]-phi2[2])+rr[2]*rr[2])/(sin(phi2[4]-phi2[2])*sin(phi2[4]-phi2[2]))-rr[i]*rr[i])*phierror[i];
188  fiterror[i] = 1 + 0 * phierror[i];
189  }
190 
191  //r-phi fitter(2D Fitter) ->calculate pt and radius of track-> input for 3D fitter.
192  A = 0, B = 0, C = 0, D = 0, E = 0, hcx = 0, hcy = 0;
193  //G=0;
194  for (int i = 0; i < nTS; i++) {
195  A += cos(phi2[i]) * cos(phi2[i]) / (fiterror[i] * fiterror[i]);
196  B += sin(phi2[i]) * sin(phi2[i]) / (fiterror[i] * fiterror[i]);
197  C += cos(phi2[i]) * sin(phi2[i]) / (fiterror[i] * fiterror[i]);
198  D += rr[i] * cos(phi2[i]) / (fiterror[i] * fiterror[i]);
199  E += rr[i] * sin(phi2[i]) / (fiterror[i] * fiterror[i]);
200  //G+=rr[i]*rr[i]/(fiterror[i]*fiterror[i]);
201  }
202  hcx = D * B - E * C; //helix center x
203  hcx /= 2 * (A * B - C * C);
204  hcy = E * A - D * C; //helix center y
205  hcy /= 2 * (A * B - C * C);
206  rho = sqrt(hcx * hcx + hcy * hcy); //radius of helix
207  myphi0 = atan2(hcy, hcx);
208  if (myphi0 < 0) myphi0 += 2 * Trg_PI;
209  //myphi0=atan(hcy/hcx);
210  //if(hcx<0 && hcy>0) myphi0 += Trg_PI;
211  //if(hcx<0 && hcy<0) myphi0 += Trg_PI;
212  //if(hcx>0 && hcy<0) myphi0 += Trg_PI*2.0;
213 
215  // double pchi2 = -2*hcx*D-2*hcy*E+G;
216  // pchi2/=nTS-2;
217 
219  //double pchi3 = 0.; //iw
220  //for(int i=0;i<nTS;i++){
221  // pchi3+=(2*(hcx*cos(phi2[i])+hcy*sin(phi2[i]))-rr[i])*(2*(hcx*cos(phi2[i])+hcy*sin(phi2[i]))-rr[i])/(fiterror[i]*fiterror[i]);
222  //}
223  //pchi3/=3;
224 }
225 
226 unsigned Fitter3DUtility::toUnsignedTdc(int tdc, int nBits)
227 {
228  bool notDone = 1;
229  int tdcMax = pow(2, nBits) - 1;
230  int resultTdc = tdc;
231  while (notDone) {
232  if (resultTdc > tdcMax) resultTdc -= (tdcMax + 1);
233  else if (resultTdc < 0) resultTdc += (tdcMax + 1);
234  else notDone = 0;
235  }
236  return (unsigned) resultTdc;
237 }
238 
239 double Fitter3DUtility::calPhi(double wirePhi, double driftLength, double rr, int lr)
240 {
241  //cout<<"rr:"<<rr<<" lr:"<<lr;
242  double result = wirePhi;
243  //cout<<" wirePhi:"<<result;
244  // time is in 2ns rms clock.
245  double t_dPhi = driftLength;
246  //cout<<" driftLength:"<<t_dPhi;
247  // Change to radian
248  // rr is cm scale.
249  t_dPhi = atan(t_dPhi / rr);
250  //cout<<" driftPhi:"<<t_dPhi;
251  // Use LR to add dPhi
252  if (lr == 1) result -= t_dPhi;
253  else if (lr == 2) result += t_dPhi;
254  //cout<<" phi:"<<result<<endl;
255  return result;
256 }
257 
258 double Fitter3DUtility::calPhi(double wirePhi, double driftTime, double eventTime, double rr, int lr)
259 {
260  // time is in 2ns rms clock.
261  double t_driftLength = (driftTime - eventTime) * 1000 / 1017.774 * 2 * 40 / 1000 / 10;
262  return calPhi(wirePhi, t_driftLength, rr, lr);
263 }
264 
265 double Fitter3DUtility::calPhi(int localId, int nWires, double driftTime, double eventTime, double rr, int lr)
266 {
267  double wirePhi = (double)localId / nWires * 4 * M_PI;
268  return Fitter3DUtility::calPhi(wirePhi, driftTime, eventTime, rr, lr);
269 }
270 
271 void Fitter3DUtility::calPhi(std::map<std::string, double> const& mConstD,
272  std::map<std::string, std::vector<double> > const& mConstV, std::map<std::string, Belle2::TRGCDCJSignal>& mSignalStorage,
273  std::map<std::string, Belle2::TRGCDCJLUT* >& mLutStorage)
274 {
275  Belle2::TRGCDCJSignalData* commonData = mSignalStorage.begin()->second.getCommonData();
276  //for(int iSt=0; iSt<4; iSt++) {cout<<"<<<tsId_"<<iSt<<">>>"<<endl; mSignalStorage["tsId_"+to_string(iSt)].dump();}
277  //for(int iSt=0; iSt<4; iSt++) {cout<<"<<<tdc_"<<iSt<<">>>"<<endl; mSignalStorage["tdc_"+to_string(iSt)].dump();}
278  //for(int iSt=0; iSt<4; iSt++) {cout<<"<<<lr_"<<iSt<<">>>"<<endl; mSignalStorage["lr_"+to_string(iSt)].dump();}
279  // Make phiFactor constants
280  if (mSignalStorage.find("phiFactor_0") == mSignalStorage.end()) {
281  for (unsigned iSt = 0; iSt < 4; iSt++) {
282  int nShiftBits = int(log(pow(2, 24) / 2 / mConstD.at("Trg_PI") * mConstV.at("nTSs")[2 * iSt + 1] *
283  mSignalStorage["phi0"].getToReal()) / log(2));
284  string t_name;
285  t_name = "phiFactor_" + to_string(iSt);
286  mSignalStorage[t_name] = Belle2::TRGCDCJSignal(2 * mConstD.at("Trg_PI") / mConstV.at("nTSs")[2 * iSt + 1],
287  mSignalStorage["phi0"].getToReal() / pow(2, nShiftBits), commonData);
288  }
289  }
290  //for(int iSt=0; iSt<4; iSt++) {cout<<"<<<phiFactor_"<<iSt<<">>>"<<endl; mSignalStorage["phiFactor_"+to_string(iSt)].dump();}
291  // wirePhi <= tsId * phiFactor
292  for (unsigned iSt = 0; iSt < 4;
293  iSt++) mSignalStorage["wirePhi_" + to_string(iSt)] <= mSignalStorage["tsId_" + to_string(iSt)] * mSignalStorage["phiFactor_" +
294  to_string(iSt)];
295  //for(int iSt=0; iSt<4; iSt++) {cout<<"<<<wirePhi_"<<iSt<<">>>"<<endl; mSignalStorage["wirePhi_"+to_string(iSt)].dump();}
296 
297  //for(int iSt=0; iSt<4; iSt++) {cout<<"<<<tdc_"<<iSt<<">>>"<<endl; mSignalStorage["tdc_"+to_string(iSt)].dump();}
298  //cout<<"<<<eventTime>>>"<<endl; mSignalStorage["eventTime"].dump();
299 
300  // Calculate driftTimeRaw (= tdc - eventTime)
301  for (unsigned iSt = 0; iSt < 4;
302  iSt++) mSignalStorage["driftTimeRaw_" + to_string(iSt)] <= mSignalStorage["tdc_" + to_string(iSt)] - mSignalStorage["eventTime"];
303 
304  //for(int iSt=0; iSt<4; iSt++) {cout<<"<<<driftTimeRaw_"<<iSt<<">>>"<<endl; mSignalStorage["driftTimeRaw_"+to_string(iSt)].dump();}
305 
306  // Confine drift time
307  // Constants for driftTime
308  if (mSignalStorage.find("maxDriftTimeLow") == mSignalStorage.end()) {
309  int maxDriftTime = 287;
310  mSignalStorage["maxDriftTimeLow"] = Belle2::TRGCDCJSignal(-512 + maxDriftTime + 1, mSignalStorage["eventTime"].getToReal(),
311  commonData);
312  mSignalStorage["maxDriftTimeHigh"] = Belle2::TRGCDCJSignal(maxDriftTime, mSignalStorage["eventTime"].getToReal(), commonData);
313  mSignalStorage["maxDriftTimeMax"] = Belle2::TRGCDCJSignal(512, mSignalStorage["eventTime"].getToReal(), commonData);
314  }
315  for (unsigned iSt = 0; iSt < 4; iSt++) {
316  string t_in1Name = "driftTimeRaw_" + to_string(iSt);
317  string t_valueName = "driftTime_c_" + to_string(iSt);
318  // Create data for ifElse
319  vector<pair<Belle2::TRGCDCJSignal, vector<pair<Belle2::TRGCDCJSignal*, Belle2::TRGCDCJSignal> > > > t_data;
320  // Compare (eventTimeValid == 0)
321  Belle2::TRGCDCJSignal t_compare = Belle2::TRGCDCJSignal::comp(mSignalStorage["eventTimeValid"], "=", Belle2::TRGCDCJSignal(0,
322  mSignalStorage["eventTimeValid"].getToReal(), commonData));
323  // Assignments (driftTime = 0)
324  vector<pair<Belle2::TRGCDCJSignal*, Belle2::TRGCDCJSignal> > t_assigns = {
325  make_pair(&mSignalStorage[t_valueName], Belle2::TRGCDCJSignal(0, mSignalStorage["eventTime"].getToReal(), commonData))
326  };
327  // Push to data.
328  t_data.push_back(make_pair(t_compare, t_assigns));
329  // Compare (driftTime < maxDriftTimeLow)
330  t_compare = Belle2::TRGCDCJSignal::comp(mSignalStorage[t_in1Name], "<", mSignalStorage["maxDriftTimeLow"]);
331  // Assignments
332  t_assigns = {
333  make_pair(&mSignalStorage[t_valueName], (mSignalStorage[t_in1Name] + mSignalStorage["maxDriftTimeMax"]).limit(mSignalStorage["maxDriftTimeLow"], mSignalStorage["maxDriftTimeHigh"]))
334  };
335  // Push to data.
336  t_data.push_back(make_pair(t_compare, t_assigns));
337  // Compare (driftTime < maxDriftTimeHigh)
338  t_compare = Belle2::TRGCDCJSignal::comp(mSignalStorage[t_in1Name], "<=", mSignalStorage["maxDriftTimeHigh"]);
339  // Assignments
340  t_assigns = {
341  make_pair(&mSignalStorage[t_valueName], mSignalStorage[t_in1Name].limit(mSignalStorage["maxDriftTimeLow"], mSignalStorage["maxDriftTimeHigh"]))
342  };
343  // Push to data.
344  t_data.push_back(make_pair(t_compare, t_assigns));
345  // Compare (else)
346  t_compare = Belle2::TRGCDCJSignal();
347  // Assignments
348  t_assigns = {
349  make_pair(&mSignalStorage[t_valueName], (mSignalStorage[t_in1Name] - mSignalStorage["maxDriftTimeMax"]).limit(mSignalStorage["maxDriftTimeLow"], mSignalStorage["maxDriftTimeHigh"]))
350  };
351  // Push to data.
352  t_data.push_back(make_pair(t_compare, t_assigns));
353  // Process ifElse data.
355  }
356  //for(int iSt=0; iSt<4; iSt++) {cout<<"<<<driftTime_c_"<<iSt<<">>>"<<endl; mSignalStorage["driftTime_c_"+to_string(iSt)].dump();}
357 
359  //for(unsigned iSt=0; iSt<4; iSt++) {
360  // if(mSignalStorage["driftTimeRaw_"+to_string(iSt)].getInt() > maxDriftTime) cout<<"driftTimeRaw_"<<iSt<<":"<<mSignalStorage["driftTimeRaw_"+to_string(iSt)].getInt()<<" is larger than maxDriftTime:"<<maxDriftTime<<" changing to "<<mSignalStorage["driftTimeRaw_"+to_string(iSt)].getInt()-512<<endl;
361  // if(mSignalStorage["driftTimeRaw_"+to_string(iSt)].getInt() < -512+maxDriftTime+1) cout<<"driftTimeRaw_"<<iSt<<":"<<mSignalStorage["driftTimeRaw_"+to_string(iSt)].getInt()<<" is smaller than -512+maxDriftTime+1:"<<-512+maxDriftTime+1<<" changed to "<<mSignalStorage["driftTimeRaw_"+to_string(iSt)].getInt()+512<<endl;
362  //}
363 
364  //for(int iSt=0; iSt<4; iSt++) {cout<<"<<<invErrorRho_"<<iSt<<">>>"<<endl; mSignalStorage["invErrorRho_"+to_string(iSt)].dump();}
365  // Remove negative driftTime
366  // Constants for driftTime
367  if (mSignalStorage.find("minDriftTime") == mSignalStorage.end()) {
368  int minDriftTime = -8;
369  mSignalStorage["minDriftTime"] = Belle2::TRGCDCJSignal(minDriftTime, mSignalStorage["eventTime"].getToReal(), commonData);
370  mSignalStorage["minDriftTimeLow"] = Belle2::TRGCDCJSignal(0, mSignalStorage["eventTime"].getToReal(), commonData);
371  }
372  for (unsigned iSt = 0; iSt < 4; iSt++) {
373  string t_in1Name = "driftTime_c_" + to_string(iSt);
374  string t_valueName = "driftTime_" + to_string(iSt);
375  string t_in2Name = "invErrorRho_" + to_string(iSt);
376  string t_value2Name = "iZError2_" + to_string(iSt);
377  string t_noneErrorName = "iZNoneError2_" + to_string(iSt);
378  // Create data for ifElse
379  vector<pair<Belle2::TRGCDCJSignal, vector<pair<Belle2::TRGCDCJSignal*, Belle2::TRGCDCJSignal> > > > t_data;
380  // Compare (driftTime_c[i]>0)
381  Belle2::TRGCDCJSignal t_compare = Belle2::TRGCDCJSignal::comp(mSignalStorage[t_in1Name], ">=", mSignalStorage["minDriftTimeLow"]);
382  // Assignments
383  vector<pair<Belle2::TRGCDCJSignal*, Belle2::TRGCDCJSignal> > t_assigns = {
384  make_pair(&mSignalStorage[t_valueName], mSignalStorage[t_in1Name].limit(mSignalStorage["minDriftTimeLow"], mSignalStorage["maxDriftTimeHigh"])),
385  make_pair(&mSignalStorage[t_value2Name], mSignalStorage[t_in2Name])
386  };
387  // Push to data.
388  t_data.push_back(make_pair(t_compare, t_assigns));
389  // Compare (driftTime_c[i]>=minDriftTime)
390  t_compare = Belle2::TRGCDCJSignal::comp(mSignalStorage[t_in1Name], ">=", mSignalStorage["minDriftTime"]);
391  // Assignments
392  t_assigns = {
393  make_pair(&mSignalStorage[t_valueName], mSignalStorage["minDriftTimeLow"]),
394  make_pair(&mSignalStorage[t_value2Name], mSignalStorage[t_in2Name])
395  };
396  // Push to data.
397  t_data.push_back(make_pair(t_compare, t_assigns));
398  // Compare (else)
399  t_compare = Belle2::TRGCDCJSignal();
400  // Assignments
401  t_assigns = {
402  make_pair(&mSignalStorage[t_valueName], mSignalStorage["minDriftTimeLow"]),
403  make_pair(&mSignalStorage[t_value2Name], mSignalStorage[t_noneErrorName])
404  };
405  // Push to data.
406  t_data.push_back(make_pair(t_compare, t_assigns));
407  // Process ifElse data.
409  }
410  //for(int iSt=0; iSt<4; iSt++) {cout<<"<<<driftTime_"<<iSt<<">>>"<<endl; mSignalStorage["driftTime_"+to_string(iSt)].dump();}
411  //for(int iSt=0; iSt<4; iSt++) {cout<<"<<<iZError2_"<<iSt<<">>>"<<endl; mSignalStorage["iZError2_"+to_string(iSt)].dump();}
412 
414  //for(unsigned iSt=0; iSt<4; iSt++) {
415  // if(mSignalStorage["driftTime_c_"+to_string(iSt)].getInt() < minDriftTime) cout<<"driftTime_c_"<<iSt<<":"<<mSignalStorage["driftTime_c_"+to_string(iSt)].getInt()<<" is smaller than minDriftTime:"<<minDriftTime<<endl;
416  // else if(mSignalStorage["driftTime_c_"+to_string(iSt)].getInt() < 0) cout<<"driftTime_c_"<<iSt<<":"<<mSignalStorage["driftTime_c_"+to_string(iSt)].getInt()<<" is smaller than 0."<<endl;
417  //}
418 
419  // Calculate minInvValue, maxInvValue for LUTs
420  if (mSignalStorage.find("invDriftPhiMin") == mSignalStorage.end()) {
421  mSignalStorage["invDriftPhiMin"] = Belle2::TRGCDCJSignal(0, mSignalStorage["driftTime_0"].getToReal(), commonData);
422  mSignalStorage["invDriftPhiMax"] = Belle2::TRGCDCJSignal(pow(2, mConstD.at("tdcBitSize")) - 1,
423  mSignalStorage["driftTime_0"].getToReal(), commonData);
424  }
425  //cout<<"<<<invDriftPhiMin>>>"<<endl; mSignalStorage["invDriftPhiMin"].dump();
426  //cout<<"<<<invDriftPhiMax>>>"<<endl; mSignalStorage["invDriftPhiMax"].dump();
427  // Generate LUT(driftPhi[i]=arctan(driftLengthFunction(driftTime))/r[i])
428  for (unsigned iSt = 0; iSt < 4; iSt++) {
429  string t_valueName = "driftPhi_" + to_string(iSt);
430  string t_inName = "driftTime_" + to_string(iSt);
431  if (mLutStorage.find(t_valueName) == mLutStorage.end()) {
432  mLutStorage[t_valueName] = new Belle2::TRGCDCJLUT(t_valueName);
433  // Lambda can not copy maps.
434  double t_parameter = mConstV.at("rr3D")[iSt];
435  mLutStorage[t_valueName]->setFloatFunction(
436  //[=](double aValue) -> double{return mConstV.at("driftLengthTableSL"+to_string(2*iSt+1))[aValue]/t_parameter;},
437  [ = ](double aValue) -> double{return atan(mConstV.at("driftLengthTableSL" + to_string(2 * iSt + 1))[aValue] / t_parameter);},
438  mSignalStorage[t_inName],
439  mSignalStorage["invDriftPhiMin"], mSignalStorage["invDriftPhiMax"], mSignalStorage["phi0"].getToReal(),
440  (int)mConstD.at("driftPhiLUTInBitSize"), (int)mConstD.at("driftPhiLUTOutBitSize"));
441  //mLutStorage[t_valueName]->makeCOE("./LutData/"+t_valueName+".coe");
442  }
443  }
446  for (unsigned iSt = 0; iSt < 4; iSt++) {
447  //cout<<"driftTime:"<<mSignalStorage["driftTime_"+to_string(iSt)].getActual()<<endl;
448  //cout<<"actual:"<<mLutStorage["driftPhi_"+to_string(iSt)]->getFloatOutput(mSignalStorage["driftTime_"+to_string(iSt)].getActual())<<endl;
449  mLutStorage["driftPhi_" + to_string(iSt)]->operate(mSignalStorage["driftTime_" + to_string(iSt)],
450  mSignalStorage["driftPhi_" + to_string(iSt)]);
451  }
452  //for(int iSt=0; iSt<4; iSt++) {cout<<"<<<driftPhi_"<<iSt<<">>>"<<endl; mSignalStorage["driftPhi_"+to_string(iSt)].dump();}
453 
454  // Calculate finePhi depending on lr(0:no hit, 1: left, 2: right, 3: not determined)
455  //for(int iSt=0; iSt<4; iSt++) {cout<<"<<<lr_"<<iSt<<">>>"<<endl; mSignalStorage["lr_"+to_string(iSt)].dump();}
456  for (unsigned iSt = 0; iSt < 4; iSt++) {
457  string t_in1Name = "lr_" + to_string(iSt);
458  string t_valueName = "finePhi_" + to_string(iSt);
459  // Create data for ifElse
460  vector<pair<Belle2::TRGCDCJSignal, vector<pair<Belle2::TRGCDCJSignal*, Belle2::TRGCDCJSignal> > > > t_data;
461  // Compare (lr == 1)
462  Belle2::TRGCDCJSignal t_compare = Belle2::TRGCDCJSignal::comp(mSignalStorage[t_in1Name], "=", Belle2::TRGCDCJSignal(1,
463  mSignalStorage[t_in1Name].getToReal(), commonData));
464  // Assignments
465  vector<pair<Belle2::TRGCDCJSignal*, Belle2::TRGCDCJSignal> > t_assigns = {
466  make_pair(&mSignalStorage[t_valueName], mSignalStorage["wirePhi_" + to_string(iSt)] - mSignalStorage["driftPhi_" + to_string(iSt)])
467  };
468  // Push to data.
469  t_data.push_back(make_pair(t_compare, t_assigns));
470  // Compare (lr == 2)
471  t_compare = Belle2::TRGCDCJSignal::comp(mSignalStorage[t_in1Name], "=", Belle2::TRGCDCJSignal(2,
472  mSignalStorage[t_in1Name].getToReal(), commonData));
473  // Assignments
474  t_assigns = {
475  make_pair(&mSignalStorage[t_valueName], mSignalStorage["wirePhi_" + to_string(iSt)] + mSignalStorage["driftPhi_" + to_string(iSt)])
476  };
477  // Push to data.
478  t_data.push_back(make_pair(t_compare, t_assigns));
479  // Compare (else)
480  t_compare = Belle2::TRGCDCJSignal();
481  // Assignments
482  t_assigns = {
483  make_pair(&mSignalStorage[t_valueName], mSignalStorage["wirePhi_" + to_string(iSt)])
484  };
485  // Push to data.
486  t_data.push_back(make_pair(t_compare, t_assigns));
487  // Process ifElse data.
489  }
490  //for(int iSt=0; iSt<4; iSt++) {cout<<"<<<finePhi_"<<iSt<<">>>"<<endl; mSignalStorage["finePhi_"+to_string(iSt)].dump();}
491 
492  // Constrain phi[i] to [-pi, pi]
493  if (mSignalStorage.find("phiMax_0") == mSignalStorage.end()) {
494  for (unsigned iSt = 0; iSt < 4; iSt++) {
495  string t_valueName = "finePhi_" + to_string(iSt);
496  string t_maxName = "phiMax_" + to_string(iSt);
497  string t_minName = "phiMin_" + to_string(iSt);
498  string t_2PiName = "phi2Pi_" + to_string(iSt);
499  mSignalStorage[t_maxName] = Belle2::TRGCDCJSignal(mConstD.at("Trg_PI"), mSignalStorage[t_valueName].getToReal(), commonData);
500  mSignalStorage[t_minName] = Belle2::TRGCDCJSignal(-mConstD.at("Trg_PI"), mSignalStorage[t_valueName].getToReal(), commonData);
501  mSignalStorage[t_2PiName] = Belle2::TRGCDCJSignal(2 * mConstD.at("Trg_PI"), mSignalStorage[t_valueName].getToReal(), commonData);
502  }
503  }
504  for (unsigned iSt = 0; iSt < 4; iSt++) {
505  string t_in1Name = "finePhi_" + to_string(iSt);
506  string t_valueName = "phi_" + to_string(iSt);
507  string t_maxName = "phiMax_" + to_string(iSt);
508  string t_minName = "phiMin_" + to_string(iSt);
509  string t_2PiName = "phi2Pi_" + to_string(iSt);
510  // Create data for ifElse
511  vector<pair<Belle2::TRGCDCJSignal, vector<pair<Belle2::TRGCDCJSignal*, Belle2::TRGCDCJSignal> > > > t_data;
512  // Compare (dPhi[i] > dPhiPiMax)
513  Belle2::TRGCDCJSignal t_compare = Belle2::TRGCDCJSignal::comp(mSignalStorage[t_in1Name], ">", mSignalStorage[t_maxName]);
514  // Assignments (dPhiPi_c[i] = dPhi[i] - dPhiPi2Pi[i])
515  vector<pair<Belle2::TRGCDCJSignal*, Belle2::TRGCDCJSignal> > t_assigns = {
516  make_pair(&mSignalStorage[t_valueName], (mSignalStorage[t_in1Name] - mSignalStorage[t_2PiName]).limit(mSignalStorage[t_minName], mSignalStorage[t_maxName])),
517  };
518  // Push to data.
519  t_data.push_back(make_pair(t_compare, t_assigns));
520  // Compare (dPhi[i] > dPhiPiMin)
521  t_compare = Belle2::TRGCDCJSignal::comp(mSignalStorage[t_in1Name], ">", mSignalStorage[t_minName]);
522  // Assignments (dPhiPi_c[i] = dPhi[i])
523  t_assigns = {
524  make_pair(&mSignalStorage[t_valueName], mSignalStorage[t_in1Name].limit(mSignalStorage[t_minName], mSignalStorage[t_maxName])),
525  };
526  // Push to data.
527  t_data.push_back(make_pair(t_compare, t_assigns));
528  // Compare (else)
529  t_compare = Belle2::TRGCDCJSignal();
530  // Assignments (dPhiPi_c[i] = dPhi[i] + dPhiPi2Pi[i])
531  t_assigns = {
532  make_pair(&mSignalStorage[t_valueName], (mSignalStorage[t_in1Name] + mSignalStorage[t_2PiName]).limit(mSignalStorage[t_minName], mSignalStorage[t_maxName])),
533  };
534  // Push to data.
535  t_data.push_back(make_pair(t_compare, t_assigns));
536  // Process ifElse data.
538  }
539  //for(int iSt=0; iSt<4; iSt++) {cout<<"<<<phi_"<<iSt<<">>>"<<endl; mSignalStorage["phi_"+to_string(iSt)].dump();}
540 
541 }
542 
543 void Fitter3DUtility::saveStereoXt(std::vector<std::vector<double> >& stXts, string const& filePrefix)
544 {
545  for (unsigned iSt = 0; iSt < stXts.size(); ++iSt) {
546  string filename = filePrefix + ".st" + to_string(iSt);
547  ofstream xtFile;
548  cout << "Fitter3DUtility::saveStereoXt() Saving xt file to " << filename << endl;
549  xtFile.open(filename);
550  for (unsigned iTick = 0; iTick < stXts[iSt].size(); ++iTick) {
551  xtFile << stXts[iSt][iTick] << endl;
552  }
553  xtFile.close();
554  }
555 }
556 
557 void Fitter3DUtility::loadStereoXt(string const& filePrefix, int nFiles, std::vector<std::vector<double> >& stXts)
558 {
559  stXts = vector<vector<double> > (nFiles);
560  for (unsigned iSt = 0; iSt < stXts.size(); ++iSt) {
561  string filename = filePrefix + ".st" + to_string(iSt);
562  ifstream xtFile;
563  xtFile.open(filename.c_str());
564  if (xtFile.fail()) {
565  cout << "Fitter3DUtility::loadStereoXt() Cannot load " << filename << endl;
566  return;
567  }
568  string line;
569  while (getline(xtFile, line)) {
570  stXts[iSt].push_back(std::stof(line));
571  }
572  xtFile.close();
573  }
574 }
575 
576 double Fitter3DUtility::stereoIdToPhi(vector<double>& stNWires, int iSt, int id)
577 {
578  return (double)id / stNWires[iSt] * 4 * M_PI;
579 }
580 
581 // rawStTSs[iSt] = [TS ID, TS LR, TS driftTime]
582 // stTSs is 9999 when not valid
583 void Fitter3DUtility::calPhiFast(map<string, vector<double> >& stGeometry, vector<vector<double> > const& stXts, int eventTimeValid,
584  int eventTime, vector<vector<int> > const& rawStTSs, vector<double>& stTSs)
585 {
586  stTSs = vector<double> (4, 9999);
587  if (eventTimeValid == 0) {
588  for (unsigned iSt = 0; iSt < 4; iSt++) {
589  stTSs[iSt] = stereoIdToPhi(stGeometry["nWires"], iSt, rawStTSs[iSt][0]);
590  }
591  } else {
592  //cout<<"eventTime: "<<eventTime;
593  for (unsigned iSt = 0; iSt < 4; iSt++) {
594  if (rawStTSs[iSt][1] != 0) {
595  // Get drift length from table.
596  int t = rawStTSs[iSt][2] - eventTime;
597  //cout<<" iSt:"<<iSt<<" rt: "<<rawStTSs[iSt][2];
598  if (t < 0) t = 0;
599  if (t > 511) t = 511;
600  double driftLength = stXts[iSt][t];
601  stTSs[iSt] = Fitter3DUtility::calPhi(stereoIdToPhi(stGeometry["nWires"], iSt, rawStTSs[iSt][0]), driftLength,
602  stGeometry["cdcRadius"][iSt], rawStTSs[iSt][1]);
603  }
604  }
605  //cout<<endl;
606  }
607 }
608 
609 double Fitter3DUtility::rotatePhi(double value, double refPhi)
610 {
611  double Trg_PI = 3.141592653589793;
612  double phiMin = -Trg_PI;
613  double phiMax = Trg_PI;
614  double result = value - refPhi;
615  bool rangeOK = 0;
616  while (rangeOK == 0) {
617  if (result > phiMax) result -= 2 * Trg_PI;
618  else if (result < phiMin) result += 2 * Trg_PI;
619  else rangeOK = 1;
620  }
621  return result;
622 }
623 
624 double Fitter3DUtility::rotatePhi(double value, int refId, int nTSs)
625 {
626  double refPhi = (double)refId / nTSs * 2 * M_PI;
627  return rotatePhi(value, refPhi);
628 }
629 
630 int Fitter3DUtility::rotateTsId(int value, int refId, int nTSs)
631 {
632  int result = value - refId;
633  bool rangeOk = 0;
634  while (rangeOk == 0) {
635  if (result >= nTSs) result -= nTSs;
636  else if (result < 0) result += nTSs;
637  else rangeOk = 1;
638  }
639  return result;
640 }
641 
643 {
644  // Rotate to [-pi,pi] range.
645  double Trg_PI = 3.141592653589793;
646  double phiMin = -Trg_PI;
647  double phiMax = Trg_PI;
648  bool rangeOK = 0;
649  while (rangeOK == 0) {
650  if (value > phiMax) value -= 2 * Trg_PI;
651  else if (value < phiMin) value += 2 * Trg_PI;
652  else rangeOK = 1;
653  }
654  // Find quadrant.
655  int result = -1;
656  if (value > Trg_PI / 2) result = 2;
657  else if (value > 0) result = 1;
658  else if (value > -Trg_PI / 2) result = 4;
659  else if (value > -Trg_PI) result = 3;
660  return result;
661 }
662 
663 
664 // rawStTSs[iSt] = [TS ID, TS LR, TS driftTime]
665 void Fitter3DUtility::setErrorFast(std::vector<std::vector<int> > const& rawStTSs, int eventTimeValid,
666  std::vector<double>& invZError2)
667 {
668  vector<double> driftZError({0.7676, 0.9753, 1.029, 1.372});
669  vector<double> wireZError({0.7676, 0.9753, 1.029, 1.372});
670  vector<double> zError(4, 9999);
671  invZError2 = vector<double> (4, 0);
672  for (unsigned iSt = 0; iSt < 4; ++iSt) {
673  if (rawStTSs[iSt][1] != 0) {
674  // Check LR and eventTime
675  if (rawStTSs[iSt][1] != 3 && eventTimeValid != 0) zError[iSt] = driftZError[iSt];
676  else zError[iSt] = wireZError[iSt];
677  // Get inverse zerror ^ 2
678  invZError2[iSt] = 1 / pow(zError[iSt], 2);
679  }
680  }
681 }
682 
683 void Fitter3DUtility::setError(std::map<std::string, double> const& mConstD,
684  std::map<std::string, std::vector<double> > const& mConstV, std::map<std::string, Belle2::TRGCDCJSignal>& mSignalStorage)
685 {
686  Belle2::TRGCDCJSignalData* commonData = mSignalStorage.begin()->second.getCommonData();
687  // Make constants for wireError,driftError,noneError
688  if (mSignalStorage.find("iZWireError2_0") == mSignalStorage.end()) {
689  for (unsigned iSt = 0; iSt < 4; iSt++) {
690  string t_name;
691  t_name = "iZWireError2_" + to_string(iSt);
692  mSignalStorage[t_name] = Belle2::TRGCDCJSignal(mConstD.at("iError2BitSize"), 1 / pow(mConstV.at("wireZError")[iSt], 2), 0,
693  mConstD.at("iError2Max"), -1, commonData);
694  t_name = "iZDriftError2_" + to_string(iSt);
695  mSignalStorage[t_name] = Belle2::TRGCDCJSignal(mConstD.at("iError2BitSize"), 1 / pow(mConstV.at("driftZError")[iSt], 2), 0,
696  mConstD.at("iError2Max"), -1, commonData);
697  t_name = "iZNoneError2_" + to_string(iSt);
698  mSignalStorage[t_name] = Belle2::TRGCDCJSignal(mConstD.at("iError2BitSize"), 0, 0, mConstD.at("iError2Max"), -1, commonData);
699  }
700  }
701  //for(int iSt=0; iSt<4; iSt++) {cout<<"<<<iZWireError2_"<<iSt<<">>>"<<endl; mSignalStorage["iZWireError2_"+to_string(iSt)].dump();}
702  //for(int iSt=0; iSt<4; iSt++) {cout<<"<<<iZDriftError2_"<<iSt<<">>>"<<endl; mSignalStorage["iZDriftError2_"+to_string(iSt)].dump();}
703  //for(int iSt=0; iSt<4; iSt++) {cout<<"<<<iZNoneError2_"<<iSt<<">>>"<<endl; mSignalStorage["iZNoneError2_"+to_string(iSt)].dump();}
704 
705  // Set error depending on lr(0:no hit, 1: left, 2: right, 3: not determined)
706  //for(int iSt=0; iSt<4; iSt++) {cout<<"<<<lr_"<<iSt<<">>>"<<endl; mSignalStorage["lr_"+to_string(iSt)].dump();}
707  for (unsigned iSt = 0; iSt < 4; iSt++) {
708  string t_in1Name = "lr_" + to_string(iSt);
709  string t_valueName = "invErrorLR_" + to_string(iSt);
710  string t_noneErrorName = "iZNoneError2_" + to_string(iSt);
711  string t_driftErrorName = "iZDriftError2_" + to_string(iSt);
712  string t_wireErrorName = "iZWireError2_" + to_string(iSt);
713  // Create data for ifElse
714  vector<pair<Belle2::TRGCDCJSignal, vector<pair<Belle2::TRGCDCJSignal*, Belle2::TRGCDCJSignal> > > > t_data;
715  // Compare (lr == 0)
716  Belle2::TRGCDCJSignal t_compare = Belle2::TRGCDCJSignal::comp(mSignalStorage[t_in1Name], "=", Belle2::TRGCDCJSignal(0,
717  mSignalStorage[t_in1Name].getToReal(), commonData));
718  // Assignments (invErrorLR <= iZNoneError2)
719  vector<pair<Belle2::TRGCDCJSignal*, Belle2::TRGCDCJSignal> > t_assigns = {
720  make_pair(&mSignalStorage[t_valueName], mSignalStorage[t_noneErrorName])
721  };
722  // Push to data.
723  t_data.push_back(make_pair(t_compare, t_assigns));
724  // Compare (lr == 3)
725  t_compare = Belle2::TRGCDCJSignal::comp(mSignalStorage[t_in1Name], "=", Belle2::TRGCDCJSignal(3,
726  mSignalStorage[t_in1Name].getToReal(), commonData));
727  // Assignments (invErrorLR <= iZWireError)
728  t_assigns = {
729  make_pair(&mSignalStorage[t_valueName], mSignalStorage[t_wireErrorName])
730  };
731  // Push to data.
732  t_data.push_back(make_pair(t_compare, t_assigns));
733  // Compare (else)
734  t_compare = Belle2::TRGCDCJSignal();
735  // Assignments (invErrorLR <= iZDriftError)
736  t_assigns = {
737  make_pair(&mSignalStorage[t_valueName], mSignalStorage[t_driftErrorName])
738  };
739  // Push to data.
740  t_data.push_back(make_pair(t_compare, t_assigns));
741  // Process ifElse data.
743  }
744  //for(int iSt=0; iSt<4; iSt++) {cout<<"<<<invErrorLR_"<<iSt<<">>>"<<endl; mSignalStorage["invErrorLR_"+to_string(iSt)].dump();}
745 
746  // Make constants for half radius of SL
747  if (mSignalStorage.find("halfRadius_0") == mSignalStorage.end()) {
748  for (unsigned iSt = 0; iSt < 4; iSt++) {
749  string t_name;
750  t_name = "halfRadius_" + to_string(iSt);
751  mSignalStorage[t_name] = Belle2::TRGCDCJSignal(mConstV.at("rr")[iSt * 2 + 1] / 2, mSignalStorage["rho"].getToReal(), commonData);
752  }
753  }
754  //for(int iSt=0; iSt<4; iSt++) {cout<<"<<<halfRadius_"<<iSt<<">>>"<<endl; mSignalStorage["halfRadius_"+to_string(iSt)].dump();}
755  // Set error depending on R(helix radius)
756  //cout<<"<<<rho>>>"<<endl; mSignalStorage["rho"].dump();
757  for (unsigned iSt = 0; iSt < 4; iSt++) {
758  string t_compareName = "halfRadius_" + to_string(iSt);
759  //string t_valueName = "iZError2_" + to_string(iSt);
760  string t_valueName = "invErrorRho_" + to_string(iSt);
761  string t_noneErrorName = "iZNoneError2_" + to_string(iSt);
762  string t_in1Name = "invErrorLR_" + to_string(iSt);
763  // Create data for ifElse
764  vector<pair<Belle2::TRGCDCJSignal, vector<pair<Belle2::TRGCDCJSignal*, Belle2::TRGCDCJSignal> > > > t_data;
765  // Compare (R < r/2)
766  Belle2::TRGCDCJSignal t_compare = Belle2::TRGCDCJSignal::comp(mSignalStorage["rho"], "<", mSignalStorage[t_compareName]);
767  // Assignments (invError <= 0)
768  vector<pair<Belle2::TRGCDCJSignal*, Belle2::TRGCDCJSignal> > t_assigns = {
769  make_pair(&mSignalStorage[t_valueName], mSignalStorage[t_noneErrorName])
770  };
771  // Push to data.
772  t_data.push_back(make_pair(t_compare, t_assigns));
773  // Compare (else)
774  t_compare = Belle2::TRGCDCJSignal();
775  // Assignments (invError <= invErrorLR)
776  t_assigns = {
777  make_pair(&mSignalStorage[t_valueName], mSignalStorage[t_in1Name])
778  };
779  // Push to data.
780  t_data.push_back(make_pair(t_compare, t_assigns));
781  // Process ifElse data.
783  }
784  //for(int iSt=0; iSt<4; iSt++) {cout<<"<<<invError_"<<iSt<<">>>"<<endl; mSignalStorage["invError_"+to_string(iSt)].dump();}
785 }
786 
787 double Fitter3DUtility::calStAxPhi(int mysign, double anglest, double ztostraw, double rr, double rho, double myphi0)
788 {
789  if (false) cout << anglest << ztostraw << endl; // Removes warnings when compileing
790 
791  double myphiz, acos_real;
792  double Trg_PI = 3.141592653589793;
793  //Find phifit-phist
794  double t_rho = rho;
795  if (rr > (2 * rho)) t_rho = rr / 2;
796  acos_real = acos(rr / (2 * t_rho));
797  if (mysign == 1) {
798  myphiz = +acos_real + myphi0;
799  } else {
800  myphiz = -acos_real + myphi0;
801  }
802  if (myphiz > Trg_PI) myphiz -= 2 * Trg_PI;
803  if (myphiz < -Trg_PI) myphiz += 2 * Trg_PI;
804 
805  return myphiz;
806 }
807 
808 double Fitter3DUtility::calDeltaPhi(int mysign, double anglest, double ztostraw, double rr, double phi2, double rho, double myphi0)
809 {
810  if (false) cout << anglest << ztostraw << endl; // Removes warnings when compileing
811 
812  double myphiz, acos_real;
813  double Trg_PI = 3.141592653589793;
814  //Find phifit-phist
815  double t_rho = rho;
816  if (rr > (2 * rho)) t_rho = rr / 2;
817  acos_real = acos(rr / (2 * t_rho));
818  if (mysign == 1) {
819  myphiz = -acos_real - myphi0 + phi2;
820  } else {
821  myphiz = +acos_real - myphi0 + phi2;
822  }
823  if (myphiz > Trg_PI) myphiz -= 2 * Trg_PI;
824  if (myphiz < -Trg_PI) myphiz += 2 * Trg_PI;
825 
826  return myphiz;
827 }
828 
829 void Fitter3DUtility::constrainRPerStSl(std::map<std::string, std::vector<double> > const& mConstV,
830  std::map<std::string, Belle2::TRGCDCJSignal>& mSignalStorage)
831 {
832  Belle2::TRGCDCJSignalData* commonData = mSignalStorage.begin()->second.getCommonData();
833  // Make constants for min and max values for each super layer.
834  if (mSignalStorage.find("rhoMin_0") == mSignalStorage.end()) {
835  for (unsigned iSt = 0; iSt < 4; iSt++) {
836  // Min value
837  string t_minName = "rhoMin_" + to_string(iSt);
838  double t_actual = (mConstV.find("rr")->second)[2 * iSt + 1] / 2;
839  double t_toReal = mSignalStorage["rho"].getToReal();
840  mSignalStorage[t_minName] = Belle2::TRGCDCJSignal(t_actual, t_toReal, commonData);
841  // Add one to prevent arccos, arcsin becoming infinity.
842  if (mSignalStorage[t_minName].getRealInt() < t_actual) {
843  t_actual += 1 * t_toReal;
844  mSignalStorage[t_minName] = Belle2::TRGCDCJSignal(t_actual, t_toReal, commonData);
845  //cout<<"[Warning] "<<t_minName<<" is too small. Adding one unit to prevent arccos output being nan."<<endl;
846  //cout<<t_minName<<".getRealInt():"<<mSignalStorage[t_minName].getRealInt()<<" is new min."<<endl;
847  }
848  // Max value
849  string t_maxName = "rhoMax_" + to_string(iSt);
850  t_actual = mSignalStorage["rho"].getMaxActual();
851  mSignalStorage[t_maxName] = Belle2::TRGCDCJSignal(t_actual, t_toReal, commonData);
852  }
853  }
854  //for(int iSt=0; iSt<4; iSt++) {cout<<"<<<rhoMin_"<<iSt<<">>>"<<endl; mSignalStorage["rhoMin_"+to_string(iSt)].dump();}
855  //for(int iSt=0; iSt<4; iSt++) {cout<<"<<<rhoMax_"<<iSt<<">>>"<<endl; mSignalStorage["rhoMax_"+to_string(iSt)].dump();}
856 
857  //cout<<"<<<rho>>>"<<endl; mSignalStorage["rho"].dump();
858  // Constrain rho to min and max per layer.
859  for (unsigned iSt = 0; iSt < 4; iSt++) {
860  string t_in1Name = "rho";
861  string t_valueName = "rho_c_" + to_string(iSt);
862  string t_maxName = "rhoMax_" + to_string(iSt);
863  string t_minName = "rhoMin_" + to_string(iSt);
864  // Create data for ifElse
865  vector<pair<Belle2::TRGCDCJSignal, vector<pair<Belle2::TRGCDCJSignal*, Belle2::TRGCDCJSignal> > > > t_data;
866  // Compare
867  Belle2::TRGCDCJSignal t_compare = Belle2::TRGCDCJSignal::comp(mSignalStorage[t_in1Name], ">", mSignalStorage[t_maxName]);
868  // Assignments
869  vector<pair<Belle2::TRGCDCJSignal*, Belle2::TRGCDCJSignal> > t_assigns = {
870  make_pair(&mSignalStorage[t_valueName], mSignalStorage[t_maxName]),
871  };
872  // Push to data.
873  t_data.push_back(make_pair(t_compare, t_assigns));
874  // Compare
875  t_compare = Belle2::TRGCDCJSignal::comp(mSignalStorage[t_in1Name], ">", mSignalStorage[t_minName]);
876  // Assignments
877  t_assigns = {
878  make_pair(&mSignalStorage[t_valueName], mSignalStorage[t_in1Name].limit(mSignalStorage[t_minName], mSignalStorage[t_maxName]))
879  };
880  // Push to data.
881  t_data.push_back(make_pair(t_compare, t_assigns));
882  // Compare
883  t_compare = Belle2::TRGCDCJSignal();
884  // Assignments
885  t_assigns = {
886  make_pair(&mSignalStorage[t_valueName], mSignalStorage[t_minName])
887  };
888  // Push to data.
889  t_data.push_back(make_pair(t_compare, t_assigns));
891  }
892  //for(int iSt=0; iSt<4; iSt++) {cout<<"<<<rho_c_"<<iSt<<">>>"<<endl; mSignalStorage["rho_c_"+to_string(iSt)].dump();}
893 
894 }
895 
896 double Fitter3DUtility::calZ(int mysign, double anglest, double ztostraw, double rr, double phi2, double rho, double myphi0)
897 {
898  double myphiz = 0, acos_real = 0, dPhiAx = 0;
899  double Trg_PI = 3.141592653589793;
900  //Find phifit-phist
901  double t_rho = rho;
902  if (rr > (2 * rho)) t_rho = rr / 2;
903  acos_real = acos(rr / (2 * t_rho));
904  if (mysign == 1) {
905  dPhiAx = -acos_real - myphi0;
906  } else {
907  dPhiAx = acos_real - myphi0;
908  }
909  myphiz = dPhiAx + phi2;
910  if (myphiz > Trg_PI) myphiz -= 2 * Trg_PI;
911  if (myphiz < -Trg_PI) myphiz += 2 * Trg_PI;
912 
913  return (ztostraw - rr * 2 * sin(myphiz / 2) / anglest);
914 }
915 
916 void Fitter3DUtility::calZ(std::map<std::string, double> const& mConstD, std::map<std::string, std::vector<double> > const& mConstV,
917  std::map<std::string, Belle2::TRGCDCJSignal>& mSignalStorage, std::map<std::string, Belle2::TRGCDCJLUT* >& mLutStorage)
918 {
919  Belle2::TRGCDCJSignalData* commonData = mSignalStorage.begin()->second.getCommonData();
920  // Calculate zz[i]
921  if (mSignalStorage.find("invPhiAxMin_0") == mSignalStorage.end()) {
922  //for(unsigned iSt=0; iSt<4; iSt++) {
923  // string t_invMinName = "invPhiAxMin_" + to_string(iSt);
924  // double t_actual = (mConstV.find("rr")->second)[2*iSt+1]/2;
925  // double t_toReal = mSignalStorage["rho"].getToReal();
926  // mSignalStorage[t_invMinName] = Belle2::TRGCDCJSignal(t_actual, t_toReal, commonData);
927  // // Add one to prevent arccos becoming infinity.
928  // if(mSignalStorage[t_invMinName].getRealInt() < t_actual) {
929  // t_actual += 1*t_toReal;
930  // mSignalStorage[t_invMinName] = Belle2::TRGCDCJSignal(t_actual, t_toReal, commonData);
931  // //cout<<"[Warning] "<<t_invMinName<<" is too small. Adding one unit to prevent arccos output being nan."<<endl;
932  // //cout<<t_invMinName<<".getRealInt():"<<mSignalStorage[t_invMinName].getRealInt()<<" is new min."<<endl;
933  // }
934  // string t_invMaxName = "invPhiAxMax_" + to_string(iSt);
935  // t_actual = mSignalStorage["rho"].getMaxActual();
936  // mSignalStorage[t_invMaxName] = Belle2::TRGCDCJSignal(t_actual, t_toReal, commonData);
937  //}
938  for (unsigned iSt = 0; iSt < 4; iSt++) {
939  string t_invMinName = "invPhiAxMin_" + to_string(iSt);
940  //double t_actual = (mConstV.find("rr")->second)[2*iSt+1]/2;
941  double t_actual = mSignalStorage["rho_c_" + to_string(iSt)].getMinActual();
942  double t_toReal = mSignalStorage["rho_c_" + to_string(iSt)].getToReal();
943  mSignalStorage[t_invMinName] = Belle2::TRGCDCJSignal(t_actual, t_toReal, commonData);
944  string t_invMaxName = "invPhiAxMax_" + to_string(iSt);
945  t_actual = mSignalStorage["rho_c_" + to_string(iSt)].getMaxActual();
946  mSignalStorage[t_invMaxName] = Belle2::TRGCDCJSignal(t_actual, t_toReal, commonData);
947  }
948  }
949  //for(int iSt=0; iSt<4; iSt++) {cout<<"<<<rho_c_"<<iSt<<">>>"<<endl; mSignalStorage["rho_c_"+to_string(iSt)].dump();}
950  //for(int iSt=0; iSt<4; iSt++) {cout<<"<<<invPhiAxMin_"<<iSt<<">>>"<<endl; mSignalStorage["invPhiAxMin_"+to_string(iSt)].dump();}
951  //for(int iSt=0; iSt<4; iSt++) {cout<<"<<<invPhiAxMax_"<<iSt<<">>>"<<endl; mSignalStorage["invPhiAxMax_"+to_string(iSt)].dump();}
952  // Generate LUT(phiAx[i]=acos(rr[i]/2/rho)).
953  for (unsigned iSt = 0; iSt < 4; iSt++) {
954  string t_valueName = "phiAx_" + to_string(iSt);
955  string t_minName = "phiAxMin_" + to_string(iSt);
956  string t_maxName = "phiAxMax_" + to_string(iSt);
957  string t_invMinName = "invPhiAxMin_" + to_string(iSt);
958  string t_invMaxName = "invPhiAxMax_" + to_string(iSt);
959  if (mLutStorage.find(t_valueName) == mLutStorage.end()) {
960  mLutStorage[t_valueName] = new Belle2::TRGCDCJLUT(t_valueName);
961  // Lambda can not copy maps.
962  double t_parameter = mConstV.at("rr3D")[iSt];
963  mLutStorage[t_valueName]->setFloatFunction(
964  [ = ](double aValue) -> double{return acos(t_parameter / 2 / aValue);},
965  //mSignalStorage["rho"],
966  mSignalStorage["rho_c_" + to_string(iSt)],
967  mSignalStorage[t_invMinName], mSignalStorage[t_invMaxName], mSignalStorage["phi0"].getToReal(),
968  (int)mConstD.at("acosLUTInBitSize"), (int)mConstD.at("acosLUTOutBitSize"));
969  //mLutStorage[t_valueName]->makeCOE("./LutData/"+t_valueName+".coe");
970  }
971  }
972  // phiAx[i] = acos(rr[i]/2/rho).
973  // Operate using LUT(phiAx[i]).
974  for (unsigned iSt = 0; iSt < 4; iSt++) {
975  // Set output name
976  string t_valueName = "phiAx_" + to_string(iSt);
977  //mLutStorage[t_valueName]->operate(mSignalStorage["rho"], mSignalStorage[t_valueName]);
978  mLutStorage[t_valueName]->operate(mSignalStorage["rho_c_" + to_string(iSt)], mSignalStorage[t_valueName]);
979  }
980  //for(int iSt=0; iSt<4; iSt++) {cout<<"<<<phiAx_"<<iSt<<">>>"<<endl; mSignalStorage["phiAx_"+to_string(iSt)].dump();}
981  //cout<<"<<<phi0>>>"<<endl; mSignalStorage["phi0"].dump();
982  //cout<<"<<<charge>>>"<<endl; mSignalStorage["charge"].dump();
983  // dPhiAx[i] = -+phiAx[i] - phi0
984  {
985  // Create data for ifElse
986  vector<pair<Belle2::TRGCDCJSignal, vector<pair<Belle2::TRGCDCJSignal*, Belle2::TRGCDCJSignal> > > > t_data;
987  // Compare
988  Belle2::TRGCDCJSignal t_compare = Belle2::TRGCDCJSignal::comp(Belle2::TRGCDCJSignal(1, mSignalStorage["charge"].getToReal(),
989  commonData), "=", mSignalStorage["charge"]);
990  // Assignments
991  vector<pair<Belle2::TRGCDCJSignal*, Belle2::TRGCDCJSignal> > t_assigns = {
992  make_pair(&mSignalStorage["dPhiAx_0"], -mSignalStorage["phiAx_0"] - mSignalStorage["phi0"]),
993  make_pair(&mSignalStorage["dPhiAx_1"], -mSignalStorage["phiAx_1"] - mSignalStorage["phi0"]),
994  make_pair(&mSignalStorage["dPhiAx_2"], -mSignalStorage["phiAx_2"] - mSignalStorage["phi0"]),
995  make_pair(&mSignalStorage["dPhiAx_3"], -mSignalStorage["phiAx_3"] - mSignalStorage["phi0"])
996  };
997  // Push to data.
998  t_data.push_back(make_pair(t_compare, t_assigns));
999  // Compare
1000  t_compare = Belle2::TRGCDCJSignal();
1001  // Assignments
1002  t_assigns = {
1003  make_pair(&mSignalStorage["dPhiAx_0"], mSignalStorage["phiAx_0"] - mSignalStorage["phi0"]),
1004  make_pair(&mSignalStorage["dPhiAx_1"], mSignalStorage["phiAx_1"] - mSignalStorage["phi0"]),
1005  make_pair(&mSignalStorage["dPhiAx_2"], mSignalStorage["phiAx_2"] - mSignalStorage["phi0"]),
1006  make_pair(&mSignalStorage["dPhiAx_3"], mSignalStorage["phiAx_3"] - mSignalStorage["phi0"])
1007  };
1008  // Push to data.
1009  t_data.push_back(make_pair(t_compare, t_assigns));
1010  // Process ifElse data.
1012  }
1013  //for(int iSt=0; iSt<4; iSt++) {cout<<"<<<dPhiAx_"<<iSt<<">>>"<<endl; mSignalStorage["dPhiAx_"+to_string(iSt)].dump();}
1014  //for(int iSt=0; iSt<4; iSt++) {cout<<"<<<phi_"<<iSt<<">>>"<<endl; mSignalStorage["phi_"+to_string(iSt)].dump();}
1015  // dPhi[i] = dPhiAx[i] + phi[i]
1016  for (unsigned iSt = 0; iSt < 4; iSt++) {
1017  mSignalStorage["dPhi_" + to_string(iSt)] <= mSignalStorage["dPhiAx_" + to_string(iSt)] + mSignalStorage["phi_" + to_string(iSt)];
1018  }
1019  //for(int iSt=0; iSt<4; iSt++) {cout<<"<<<dPhi_"<<iSt<<">>>"<<endl; mSignalStorage["dPhi_"+to_string(iSt)].dump();}
1020 
1021  // Rotate dPhi to [-pi, pi]
1022  if (mSignalStorage.find("dPhiPiMax_0") == mSignalStorage.end()) {
1023  for (unsigned iSt = 0; iSt < 4; iSt++) {
1024  string t_valueName = "dPhi_" + to_string(iSt);
1025  string t_maxName = "dPhiPiMax_" + to_string(iSt);
1026  string t_minName = "dPhiPiMin_" + to_string(iSt);
1027  string t_2PiName = "dPhiPi2Pi_" + to_string(iSt);
1028  mSignalStorage[t_maxName] = Belle2::TRGCDCJSignal(mConstD.at("Trg_PI"), mSignalStorage[t_valueName].getToReal(), commonData);
1029  mSignalStorage[t_minName] = Belle2::TRGCDCJSignal(-mConstD.at("Trg_PI"), mSignalStorage[t_valueName].getToReal(), commonData);
1030  mSignalStorage[t_2PiName] = Belle2::TRGCDCJSignal(2 * mConstD.at("Trg_PI"), mSignalStorage[t_valueName].getToReal(), commonData);
1031  }
1032  }
1033  for (unsigned iSt = 0; iSt < 4; iSt++) {
1034  string t_in1Name = "dPhi_" + to_string(iSt);
1035  string t_valueName = "dPhiPi_c_" + to_string(iSt);
1036  string t_maxName = "dPhiPiMax_" + to_string(iSt);
1037  string t_minName = "dPhiPiMin_" + to_string(iSt);
1038  string t_2PiName = "dPhiPi2Pi_" + to_string(iSt);
1039  // Create data for ifElse
1040  vector<pair<Belle2::TRGCDCJSignal, vector<pair<Belle2::TRGCDCJSignal*, Belle2::TRGCDCJSignal> > > > t_data;
1041  // Compare (dPhi[i] > dPhiPiMax)
1042  Belle2::TRGCDCJSignal t_compare = Belle2::TRGCDCJSignal::comp(mSignalStorage[t_in1Name], ">", mSignalStorage[t_maxName]);
1043  // Assignments (dPhiPi_c[i] = dPhi[i] - dPhiPi2Pi[i])
1044  vector<pair<Belle2::TRGCDCJSignal*, Belle2::TRGCDCJSignal> > t_assigns = {
1045  make_pair(&mSignalStorage[t_valueName], (mSignalStorage[t_in1Name] - mSignalStorage[t_2PiName]).limit(mSignalStorage[t_minName], mSignalStorage[t_maxName])),
1046  };
1047  // Push to data.
1048  t_data.push_back(make_pair(t_compare, t_assigns));
1049  // Compare (dPhi[i] > dPhiPiMin)
1050  t_compare = Belle2::TRGCDCJSignal::comp(mSignalStorage[t_in1Name], ">", mSignalStorage[t_minName]);
1051  // Assignments (dPhiPi_c[i] = dPhi[i])
1052  t_assigns = {
1053  make_pair(&mSignalStorage[t_valueName], mSignalStorage[t_in1Name].limit(mSignalStorage[t_minName], mSignalStorage[t_maxName])),
1054  };
1055  // Push to data.
1056  t_data.push_back(make_pair(t_compare, t_assigns));
1057  // Compare (else)
1058  t_compare = Belle2::TRGCDCJSignal();
1059  // Assignments (dPhiPi_c[i] = dPhi[i] + dPhiPi2Pi[i])
1060  t_assigns = {
1061  make_pair(&mSignalStorage[t_valueName], (mSignalStorage[t_in1Name] + mSignalStorage[t_2PiName]).limit(mSignalStorage[t_minName], mSignalStorage[t_maxName])),
1062  };
1063  // Push to data.
1064  t_data.push_back(make_pair(t_compare, t_assigns));
1065  // Process ifElse data.
1067  }
1068  //for(int iSt=0; iSt<4; iSt++) {cout<<"<<<dPhiPi_c_"<<iSt<<">>>"<<endl; mSignalStorage["dPhiPi_c_"+to_string(iSt)].dump();}
1069 
1070  // Calculate dPhiMax[i], dPhiMin[i]=0
1071  if (mSignalStorage.find("dPhiMax_0") == mSignalStorage.end()) {
1072  for (unsigned iSt = 0; iSt < 4; iSt++) {
1073  string t_maxName = "dPhiMax_" + to_string(iSt);
1074  string t_minName = "dPhiMin_" + to_string(iSt);
1075  string t_valueName = "dPhi_" + to_string(iSt);
1076  double t_value = 2 * mConstD.at("Trg_PI") * fabs(mConstV.at("nShift")[iSt]) / (mConstV.at("nWires")[2 * iSt + 1]);
1077  mSignalStorage[t_maxName] = Belle2::TRGCDCJSignal(t_value, mSignalStorage[t_valueName].getToReal(), commonData);
1078  mSignalStorage[t_minName] = Belle2::TRGCDCJSignal(0, mSignalStorage[t_valueName].getToReal(), commonData);
1079  }
1080  }
1081  // Constrain dPhiPi to [0, 2*pi*#shift/#holes]
1082  for (unsigned iSt = 0; iSt < 4; iSt++) {
1083  string t_maxName = "dPhiMax_" + to_string(iSt);
1084  string t_minName = "dPhiMin_" + to_string(iSt);
1085  string t_valueName = "dPhi_c_" + to_string(iSt);
1086  string t_in1Name = "dPhiPi_c_" + to_string(iSt);
1087  // For SL1, SL5
1088  if (iSt % 2 == 0) {
1089  // Create data for ifElse
1090  vector<pair<Belle2::TRGCDCJSignal, vector<pair<Belle2::TRGCDCJSignal*, Belle2::TRGCDCJSignal> > > > t_data;
1091  // Compare
1092  Belle2::TRGCDCJSignal t_compare = Belle2::TRGCDCJSignal::comp(mSignalStorage[t_in1Name], ">", Belle2::TRGCDCJSignal(0,
1093  mSignalStorage[t_in1Name].getToReal(), commonData));
1094  // Assignments
1095  vector<pair<Belle2::TRGCDCJSignal*, Belle2::TRGCDCJSignal> > t_assigns = {
1096  make_pair(&mSignalStorage[t_valueName], mSignalStorage[t_minName]),
1097  };
1098  // Push to data.
1099  t_data.push_back(make_pair(t_compare, t_assigns));
1100  // Compare
1101  t_compare = Belle2::TRGCDCJSignal::comp(mSignalStorage[t_in1Name], ">", -mSignalStorage[t_maxName]);
1102  // Assignments
1103  t_assigns = {
1104  make_pair(&mSignalStorage[t_valueName], Belle2::TRGCDCJSignal::absolute(mSignalStorage[t_in1Name]).limit(mSignalStorage[t_minName], mSignalStorage[t_maxName])),
1105  };
1106  // Push to data.
1107  t_data.push_back(make_pair(t_compare, t_assigns));
1108  // Compare
1109  t_compare = Belle2::TRGCDCJSignal();
1110  // Assignments
1111  t_assigns = {
1112  make_pair(&mSignalStorage[t_valueName], mSignalStorage[t_maxName]),
1113  };
1114  // Push to data.
1115  t_data.push_back(make_pair(t_compare, t_assigns));
1116  // Process ifElse data.
1118  // For SL3, SL7
1119  } else {
1120  // Create data for ifElse
1121  vector<pair<Belle2::TRGCDCJSignal, vector<pair<Belle2::TRGCDCJSignal*, Belle2::TRGCDCJSignal> > > > t_data;
1122  // Compare (dPhi > dPhiMax)
1123  Belle2::TRGCDCJSignal t_compare = Belle2::TRGCDCJSignal::comp(mSignalStorage[t_in1Name], ">", mSignalStorage[t_maxName]);
1124  // Assignments (dPhi_c = dPhiMax)
1125  vector<pair<Belle2::TRGCDCJSignal*, Belle2::TRGCDCJSignal> > t_assigns = {
1126  make_pair(&mSignalStorage[t_valueName], mSignalStorage[t_maxName]),
1127  };
1128  // Push to data.
1129  t_data.push_back(make_pair(t_compare, t_assigns));
1130  // Compare (dPhi > 0)
1131  t_compare = Belle2::TRGCDCJSignal::comp(mSignalStorage[t_in1Name], ">", Belle2::TRGCDCJSignal(0,
1132  mSignalStorage[t_in1Name].getToReal(), commonData));
1133  // Assignments (dPhi_c = dPhi)
1134  t_assigns = {
1135  make_pair(&mSignalStorage[t_valueName], Belle2::TRGCDCJSignal::absolute(mSignalStorage[t_in1Name]).limit(mSignalStorage[t_minName], mSignalStorage[t_maxName])),
1136  };
1137  // Push to data.
1138  t_data.push_back(make_pair(t_compare, t_assigns));
1139  // Compare (else) => (dPhi > -Pi)
1140  t_compare = Belle2::TRGCDCJSignal();
1141  // Assignments (dPhi_c = dPhiMin)
1142  t_assigns = {
1143  make_pair(&mSignalStorage[t_valueName], mSignalStorage[t_minName]),
1144  };
1145  // Push to data.
1146  t_data.push_back(make_pair(t_compare, t_assigns));
1147  // Process ifElse.
1149  }
1150  }
1151 
1152  //for(int iSt=0; iSt<4; iSt++) {cout<<"<<<dPhiMax_"<<iSt<<">>>"<<endl; mSignalStorage["dPhiMax_"+to_string(iSt)].dump();}
1153  //for(int iSt=0; iSt<4; iSt++) {cout<<"<<<dPhiMin_"<<iSt<<">>>"<<endl; mSignalStorage["dPhiMin_"+to_string(iSt)].dump();}
1154  //for(int iSt=0; iSt<4; iSt++) {cout<<"<<<dPhi_c_"<<iSt<<">>>"<<endl; mSignalStorage["dPhi_c_"+to_string(iSt)].dump();}
1155  // Calculate minInvValue, maxInvValue for LUTs
1156  if (mSignalStorage.find("invZMin_0") == mSignalStorage.end()) {
1157  for (unsigned iSt = 0; iSt < 4; iSt++) {
1158  string t_minName = "invZMin_" + to_string(iSt);
1159  string t_maxName = "invZMax_" + to_string(iSt);
1160  string t_valueName = "dPhi_c_" + to_string(iSt);
1161  unsigned long long t_int = mSignalStorage[t_valueName].getMinInt();
1162  double t_toReal = mSignalStorage[t_valueName].getToReal();
1163  double t_actual = mSignalStorage[t_valueName].getMinActual();
1164  mSignalStorage[t_minName] = Belle2::TRGCDCJSignal(t_int, t_toReal, t_int, t_int, t_actual, t_actual, t_actual, -1, commonData);
1165  t_int = mSignalStorage[t_valueName].getMaxInt();
1166  t_actual = mSignalStorage[t_valueName].getMaxActual();
1167  mSignalStorage[t_maxName] = Belle2::TRGCDCJSignal(t_int, t_toReal, t_int, t_int, t_actual, t_actual, t_actual, -1, commonData);
1168  }
1169  }
1170  // Generate LUT(zz[i] = ztostraw[0]-+rr[0]*2*sin(dPhi_c[i]/2)/angleSt[i], -+ depends on SL)
1171  for (unsigned iSt = 0; iSt < 4; iSt++) {
1172  string t_inputName = "dPhi_c_" + to_string(iSt);
1173  string t_outputName = "zz_" + to_string(iSt);
1174  string t_invMinName = "invZMin_" + to_string(iSt);
1175  string t_invMaxName = "invZMax_" + to_string(iSt);
1176 
1177  if (mLutStorage.find(t_outputName) == mLutStorage.end()) {
1178  mLutStorage[t_outputName] = new Belle2::TRGCDCJLUT(t_outputName);
1179  // Lambda can not copy maps.
1180  double t_zToStraw = mConstV.at("zToStraw")[iSt];
1181  double t_rr3D = mConstV.at("rr3D")[iSt];
1182  double t_angleSt = mConstV.at("angleSt")[iSt];
1183 
1184  if (iSt % 2 == 0) {
1185  mLutStorage[t_outputName]->setFloatFunction(
1186  [ = ](double aValue) -> double{return t_zToStraw + t_rr3D * 2 * sin(aValue / 2) / t_angleSt;},
1187  mSignalStorage[t_inputName],
1188  mSignalStorage[t_invMinName], mSignalStorage[t_invMaxName], mSignalStorage["rho"].getToReal(),
1189  (int) mConstD.at("zLUTInBitSize"), (int) mConstD.at("zLUTOutBitSize"));
1190  } else {
1191  mLutStorage[t_outputName]->setFloatFunction(
1192  [ = ](double aValue) -> double{return t_zToStraw - t_rr3D * 2 * sin(aValue / 2) / t_angleSt;},
1193  mSignalStorage[t_inputName],
1194  mSignalStorage[t_invMinName], mSignalStorage[t_invMaxName], mSignalStorage["rho"].getToReal(),
1195  (int) mConstD.at("zLUTInBitSize"), (int) mConstD.at("zLUTOutBitSize"));
1196  }
1197  //mLutStorage[t_outputName]->makeCOE("./LutData/"+t_outputName+".coe");
1198  }
1199  }
1200  // zz[i] = ztostraw[0]-+rr[0]*2*sin(dPhi_c[i]/2)/angleSt[i], -+ depends on SL)
1201  // Operate using LUT(zz[i]).
1202  for (unsigned iSt = 0; iSt < 4; iSt++) {
1203  string t_outputName = "zz_" + to_string(iSt);
1204  string t_inputName = "dPhi_c_" + to_string(iSt);
1205  mLutStorage[t_outputName]->operate(mSignalStorage[t_inputName], mSignalStorage[t_outputName]);
1206  }
1207  //for(int iSt=0; iSt<4; iSt++) {cout<<"<<<zz_"<<iSt<<">>>"<<endl; mSignalStorage["zz_"+to_string(iSt)].dump();}
1208 }
1209 double Fitter3DUtility::calS(double rho, double rr)
1210 {
1211  double result;
1212  result = rho * 2 * asin(rr / 2 / rho);
1213  return result;
1214 }
1215 
1216 void Fitter3DUtility::calS(std::map<std::string, double> const& mConstD, std::map<std::string, std::vector<double> > const& mConstV,
1217  std::map<std::string, Belle2::TRGCDCJSignal>& mSignalStorage, std::map<std::string, Belle2::TRGCDCJLUT* >& mLutStorage)
1218 {
1219  Belle2::TRGCDCJSignalData* commonData = mSignalStorage.begin()->second.getCommonData();
1220  // Calculate minInvValue, maxInvValue for LUTs
1221  if (mSignalStorage.find("invArcSMin_0") == mSignalStorage.end()) {
1222  //for(unsigned iSt=0; iSt<4; iSt++) {
1223  // string t_invMinName = "invArcSMin_" + to_string(iSt);
1224  // double t_actual = mSignalStorage["rho"].getMaxActual();
1225  // double t_toReal = mSignalStorage["rho"].getToReal();
1226  // mSignalStorage[t_invMinName] = Belle2::TRGCDCJSignal(t_actual, t_toReal, commonData);
1227  // string t_invMaxName = "invArcSMax_" + to_string(iSt);
1228  // t_actual = (mConstV.find("rr")->second)[2*iSt+1]/2;
1229  // mSignalStorage[t_invMaxName] = Belle2::TRGCDCJSignal(t_actual, t_toReal, commonData);
1230  // // Subtract one to prevent arcsin becoming infinity.
1231  // if(mSignalStorage[t_invMaxName].getRealInt() < t_actual) {
1232  // t_actual += 1*t_toReal;
1233  // mSignalStorage[t_invMaxName] = Belle2::TRGCDCJSignal(t_actual, t_toReal, commonData);
1234  // //cout<<"[Warning] "<<t_invMaxName<<" is too small. Adding one unit to prevent arcsin output being nan."<<endl;
1235  // //cout<<t_invMaxName<<".getRealInt():"<<mSignalStorage[t_invMaxName].getRealInt()<<" is new max."<<endl;
1236  // }
1237  //}
1238  for (unsigned iSt = 0; iSt < 4; iSt++) {
1239  string t_invMinName = "invArcSMin_" + to_string(iSt);
1240  //double t_actual = (mConstV.find("rr")->second)[2*iSt+1]/2;
1241  double t_actual = mSignalStorage["rho_c_" + to_string(iSt)].getMaxActual();
1242  double t_toReal = mSignalStorage["rho_c_" + to_string(iSt)].getToReal();
1243  mSignalStorage[t_invMinName] = Belle2::TRGCDCJSignal(t_actual, t_toReal, commonData);
1244  string t_invMaxName = "invArcSMax_" + to_string(iSt);
1245  t_actual = mSignalStorage["rho_c_" + to_string(iSt)].getMinActual();
1246  mSignalStorage[t_invMaxName] = Belle2::TRGCDCJSignal(t_actual, t_toReal, commonData);
1247  }
1248  }
1249  //for(int iSt=0; iSt<4; iSt++) {cout<<"<<<invArcSMin_"<<iSt<<">>>"<<endl; mSignalStorage["invArcSMin_"+to_string(iSt)].dump();}
1250  //for(int iSt=0; iSt<4; iSt++) {cout<<"<<<invArcSMax_"<<iSt<<">>>"<<endl; mSignalStorage["invArcSMax_"+to_string(iSt)].dump();}
1251 
1252  // Generate LUT(arcS[i]=2*rho*asin(rr[i]/2/rho).
1253  for (unsigned iSt = 0; iSt < 4; iSt++) {
1254  string t_valueName = "arcS_" + to_string(iSt);
1255  string t_invMinName = "invArcSMin_" + to_string(iSt);
1256  string t_invMaxName = "invArcSMax_" + to_string(iSt);
1257  if (mLutStorage.find(t_valueName) == mLutStorage.end()) {
1258  mLutStorage[t_valueName] = new Belle2::TRGCDCJLUT(t_valueName);
1259  // Lambda can not copy maps.
1260  double t_parameter = mConstV.at("rr3D")[iSt];
1261  mLutStorage[t_valueName]->setFloatFunction(
1262  [ = ](double aValue) -> double{return 2 * aValue * asin(t_parameter / 2 / aValue);},
1263  //mSignalStorage["rho"],
1264  mSignalStorage["rho_c_" + to_string(iSt)],
1265  mSignalStorage[t_invMinName], mSignalStorage[t_invMaxName], mSignalStorage["rho"].getToReal(),
1266  (int)mConstD.at("acosLUTInBitSize"), (int)mConstD.at("acosLUTOutBitSize"));
1267  //mLutStorage[t_valueName]->makeCOE("./LutData/"+t_valueName+".coe");
1268  }
1269  }
1270  // arcS[i]=2*rho*asin(rr[i]/2/rho).
1271  // Operate using LUT(arcS[i]).
1272  for (unsigned iSt = 0; iSt < 4; iSt++) {
1273  // Set output name
1274  string t_valueName = "arcS_" + to_string(iSt);
1275  //mLutStorage[t_valueName]->operate(mSignalStorage["rho"], mSignalStorage[t_valueName]);
1276  mLutStorage[t_valueName]->operate(mSignalStorage["rho_c_" + to_string(iSt)], mSignalStorage[t_valueName]);
1277  }
1278  //for(int iSt=0; iSt<4; iSt++) {cout<<"<<<arcS_"<<iSt<<">>>"<<endl; mSignalStorage["arcS_"+to_string(iSt)].dump();}
1279 }
1280 
1281 double Fitter3DUtility::calDen(double* arcS, double* zError)
1282 {
1283  double t_sum1 = 0;
1284  double t_sumSS = 0;
1285  double t_sumS = 0;
1286  for (unsigned iSt = 0; iSt < 4; iSt++) {
1287  t_sum1 += pow(1 / zError[iSt], 2);
1288  t_sumSS += pow(arcS[iSt] / zError[iSt], 2);
1289  t_sumS += arcS[iSt] / pow(zError[iSt], 2);
1290  }
1291  return t_sum1 * t_sumSS - pow(t_sumS, 2);
1292 }
1293 
1294 double Fitter3DUtility::calDenWithIZError(double* arcS, double* iZError)
1295 {
1296  double t_sum1 = 0;
1297  double t_sumSS = 0;
1298  double t_sumS = 0;
1299  for (unsigned iSt = 0; iSt < 4; iSt++) {
1300  t_sum1 += pow(iZError[iSt], 2);
1301  t_sumSS += pow(arcS[iSt] * iZError[iSt], 2);
1302  t_sumS += arcS[iSt] * pow(iZError[iSt], 2);
1303  }
1304  return t_sum1 * t_sumSS - pow(t_sumS, 2);
1305 }
1306 
1307 
1308 void Fitter3DUtility::rSFit(double* iezz2, double* arcS, double* zz, double& z0, double& cot, double& zchi2)
1309 {
1310 
1311  double ss = 0, sx = 0, sxx = 0, cotnum = 0, z0num = 0;
1312  double z0nump1[4], z0nump2[4];
1313  double z0den, iz0den;
1314 
1315  for (unsigned i = 0; i < 4; i++) {
1316  ss += iezz2[i];
1317  sx += arcS[i] * iezz2[i];
1318  sxx += arcS[i] * arcS[i] * iezz2[i];
1319  }
1320 
1321  for (unsigned i = 0; i < 4; i++) {
1322  cotnum += (ss * arcS[i] - sx) * iezz2[i] * zz[i];
1323  z0nump1[i] = sxx - sx * arcS[i];
1324  z0nump2[i] = z0nump1[i] * iezz2[i] * zz[i];
1325  z0num += z0nump2[i];
1326  }
1327  z0den = (ss * sxx) - (sx * sx);
1328  iz0den = 1. / z0den;
1329  z0num *= iz0den;
1330  cotnum *= iz0den;
1331  z0 = z0num;
1332  cot = cotnum;
1333 
1334  // Count number of total TS hits.
1335  int nTSHits = 0;
1336  for (unsigned iSt = 0; iSt < 4; iSt++) {
1337  if (iezz2[iSt] != 0) nTSHits++;
1338  }
1339  // Calculate chi2 of z0
1340  zchi2 = 0.;
1341  for (unsigned i = 0; i < 4; i++) {
1342  zchi2 += (zz[i] - z0 - cot * arcS[i]) * (zz[i] - z0 - cot * arcS[i]) * iezz2[i];
1343  }
1344  zchi2 /= (nTSHits - 2);
1345 
1346 }
1347 
1348 void Fitter3DUtility::rSFit2(double* iezz21, double* iezz22, double* arcS, double* zz, int* lutv, double& z0, double& cot,
1349  double& zchi2)
1350 {
1351 // cout << "rs2" << endl;
1352 
1353  double ss = 0, sx = 0, sxx = 0, cotnum = 0, z0num = 0;
1354  double z0nump1[4], z0nump2[4];
1355  double z0den, iz0den;
1356 
1357  for (unsigned i = 0; i < 4; i++) {
1358  if (lutv[i] == 2) {
1359  ss += iezz21[i];
1360  sx += arcS[i] * iezz21[i];
1361  sxx += arcS[i] * arcS[i] * iezz21[i];
1362  } else {
1363  ss += iezz22[i];
1364  sx += arcS[i] * iezz22[i];
1365  sxx += arcS[i] * arcS[i] * iezz22[i];
1366  }
1367  }
1368 
1369  for (unsigned i = 0; i < 4; i++) {
1370  if (lutv[i] == 2) {
1371  cotnum += (ss * arcS[i] - sx) * iezz21[i] * zz[i];
1372  z0nump1[i] = sxx - sx * arcS[i];
1373  z0nump2[i] = z0nump1[i] * iezz21[i] * zz[i];
1374  z0num += z0nump2[i];
1375  } else {
1376  cotnum += (ss * arcS[i] - sx) * iezz22[i] * zz[i];
1377  z0nump1[i] = sxx - sx * arcS[i];
1378  z0nump2[i] = z0nump1[i] * iezz22[i] * zz[i];
1379  z0num += z0nump2[i];
1380  }
1381  }
1382  z0den = (ss * sxx) - (sx * sx);
1383  iz0den = 1. / z0den;
1384  z0num *= iz0den;
1385  cotnum *= iz0den;
1386  z0 = z0num;
1387  cot = cotnum;
1388 
1389  // Calculate chi2 of z0
1390 // double zchi2 = 0.;
1391  for (unsigned i = 0; i < 4; i++) {
1392  if (lutv[i] == 2) {
1393  zchi2 += (zz[i] - z0 - cot * arcS[i]) * (zz[i] - z0 - cot * arcS[i]) * iezz21[i];
1394  } else {
1395  zchi2 += (zz[i] - z0 - cot * arcS[i]) * (zz[i] - z0 - cot * arcS[i]) * iezz22[i];
1396  }
1397  }
1398  zchi2 /= (4 - 2);
1399 }
1400 
1401 void Fitter3DUtility::rSFit(std::map<std::string, double> const& mConstD,
1402  std::map<std::string, std::vector<double> > const& mConstV, std::map<std::string, Belle2::TRGCDCJSignal>& mSignalStorage,
1403  std::map<std::string, Belle2::TRGCDCJLUT* >& mLutStorage)
1404 {
1405  Belle2::TRGCDCJSignalData* commonData = mSignalStorage.begin()->second.getCommonData();
1406  // sum1_p1_0 = iZError2[0] + iZError2[1]
1407  mSignalStorage["sum1_p1_0"] <= mSignalStorage["iZError2_0"] + mSignalStorage["iZError2_1"];
1408  // sum1_p1_1 = iZError2[2] + iZError2[3]
1409  mSignalStorage["sum1_p1_1"] <= mSignalStorage["iZError2_2"] + mSignalStorage["iZError2_3"];
1410  //for(int iSt=0; iSt<2; iSt++) {cout<<"<<<sum1_p1_"<<iSt<<">>>"<<endl; mSignalStorage["sum1_p1_"+to_string(iSt)].dump();}
1411  // sum1 = sum1_p1[0] + sum1_p1[1]
1412  mSignalStorage["sum1"] <= mSignalStorage["sum1_p1_0"] + mSignalStorage["sum1_p1_1"];
1413  //cout<<"<<<sum1>>>"<<endl; mSignalStorage["sum1"].dump();
1414  // sumS_p1[i] = arcS[i] * iZError2[i]
1415  for (unsigned iSt = 0; iSt < 4; iSt++) {
1416  mSignalStorage["sumS_p1_" + to_string(iSt)] <= mSignalStorage["arcS_" + to_string(iSt)] * mSignalStorage["iZError2_" + to_string(
1417  iSt)];
1418  }
1419  // sumSS_p1[i] = arcS[i] * arcS[i]
1420  for (unsigned iSt = 0; iSt < 4; iSt++) {
1421  mSignalStorage["sumSS_p1_" + to_string(iSt)] <= mSignalStorage["arcS_" + to_string(iSt)] * mSignalStorage["arcS_" + to_string(iSt)];
1422  }
1423  // cotNumS_p1[i] = sum1 * arcS[i]
1424  for (unsigned iSt = 0; iSt < 4; iSt++) {
1425  mSignalStorage["cotNumS_p1_" + to_string(iSt)] <= mSignalStorage["sum1"] * mSignalStorage["arcS_" + to_string(iSt)];
1426  }
1427  //for(int iSt=0; iSt<4; iSt++) {cout<<"<<<sumS_p1_"<<iSt<<">>>"<<endl; mSignalStorage["sumS_p1_"+to_string(iSt)].dump();}
1428  //for(int iSt=0; iSt<4; iSt++) {cout<<"<<<sumSS_p1_"<<iSt<<">>>"<<endl; mSignalStorage["sumSS_p1_"+to_string(iSt)].dump();}
1429  //for(int iSt=0; iSt<4; iSt++) {cout<<"<<<cotNumS_p1_"<<iSt<<">>>"<<endl; mSignalStorage["cotNumS_p1_"+to_string(iSt)].dump();}
1430  // sumS_p2[0] = sumS_p1[0] + sumS_p1[1]
1431  mSignalStorage["sumS_p2_0"] <= mSignalStorage["sumS_p1_0"] + mSignalStorage["sumS_p1_1"];
1432  // sumS_p2[1] = sumS_p1[2] + sumS_p1[3]
1433  mSignalStorage["sumS_p2_1"] <= mSignalStorage["sumS_p1_2"] + mSignalStorage["sumS_p1_3"];
1434  // sumSS_p2[i] = sumSS_p1[i] * iZError2[i]
1435  for (unsigned iSt = 0; iSt < 4; iSt++) {
1436  mSignalStorage["sumSS_p2_" + to_string(iSt)] <= mSignalStorage["sumSS_p1_" + to_string(iSt)] * mSignalStorage["iZError2_" +
1437  to_string(iSt)];
1438  }
1439  //for(int iSt=0; iSt<2; iSt++) {cout<<"<<<sumS_p2_"<<iSt<<">>>"<<endl; mSignalStorage["sumS_p2_"+to_string(iSt)].dump();}
1440  //for(int iSt=0; iSt<4; iSt++) {cout<<"<<<sumSS_p2_"<<iSt<<">>>"<<endl; mSignalStorage["sumSS_p2_"+to_string(iSt)].dump();}
1441  // sumS = sumS_p2[0] + sumS_p2[1]
1442  mSignalStorage["sumS"] <= mSignalStorage["sumS_p2_0"] + mSignalStorage["sumS_p2_1"];
1443  // sumSS_p3[0] = sumSS_p2[0] + sumSS_p2[1]
1444  mSignalStorage["sumSS_p3_0"] <= mSignalStorage["sumSS_p2_0"] + mSignalStorage["sumSS_p2_1"];
1445  // sumSS_p3[1] = sumSS_p2[2] + sumSS_p2[3]
1446  mSignalStorage["sumSS_p3_1"] <= mSignalStorage["sumSS_p2_2"] + mSignalStorage["sumSS_p2_3"];
1447  //cout<<"<<<sumS>>>"<<endl; mSignalStorage["sumS"].dump();
1448  //for(int iSt=0; iSt<2; iSt++) {cout<<"<<<sumSS_p3_"<<iSt<<">>>"<<endl; mSignalStorage["sumSS_p3_"+to_string(iSt)].dump();}
1449  //cout<<"===================PIPELINE7============================"<<endl;
1450  // z0NumS_p1[i] = arcS[i] * sumS
1451  for (unsigned iSt = 0; iSt < 4; iSt++) {
1452  mSignalStorage["z0NumS_p1_" + to_string(iSt)] <= mSignalStorage["arcS_" + to_string(iSt)] * mSignalStorage["sumS"];
1453  }
1454  // den_p1 = sumS * sumS
1455  mSignalStorage["den_p1"] <= mSignalStorage["sumS"] * mSignalStorage["sumS"];
1456  // cotNumS[i] = cotNumS_p1[i] - sumS
1457  for (unsigned iSt = 0; iSt < 4; iSt++) {
1458  mSignalStorage["cotNumS_" + to_string(iSt)] <= mSignalStorage["cotNumS_p1_" + to_string(iSt)] - mSignalStorage["sumS"];
1459  }
1460  // sumSS = sumSS_p3[0] + sumSS_p3[1]
1461  mSignalStorage["sumSS"] <= mSignalStorage["sumSS_p3_0"] + mSignalStorage["sumSS_p3_1"];
1462  // zxiZError[i] = zz[i] * iZError2[i]
1463  for (unsigned iSt = 0; iSt < 4; iSt++) {
1464  mSignalStorage["zxiZError_" + to_string(iSt)] <= mSignalStorage["zz_" + to_string(iSt)] * mSignalStorage["iZError2_" + to_string(
1465  iSt)];
1466  }
1467  //for(int iSt=0; iSt<4; iSt++) {cout<<"<<<z0NumS_p1_"<<iSt<<">>>"<<endl; mSignalStorage["z0NumS_p1_"+to_string(iSt)].dump();}
1468  //cout<<"<<<den_p1>>>"<<endl; mSignalStorage["den_p1"].dump();
1469  //for(int iSt=0; iSt<4; iSt++) {cout<<"<<<cotNumS_"<<iSt<<">>>"<<endl; mSignalStorage["cotNumS_"+to_string(iSt)].dump();}
1470  //cout<<"<<<sumSS>>>"<<endl; mSignalStorage["sumSS"].dump();
1471  //for(int iSt=0; iSt<4; iSt++) {cout<<"<<<zxiZError_"<<iSt<<">>>"<<endl; mSignalStorage["zxiZError_"+to_string(iSt)].dump();}
1472  // den_p2 = sum1 * sumSS
1473  mSignalStorage["den_p2"] <= mSignalStorage["sum1"] * mSignalStorage["sumSS"];
1474  // z0NumS[i] =sumSS - z0NumS_p1[i]
1475  for (unsigned iSt = 0; iSt < 4; iSt++) {
1476  mSignalStorage["z0NumS_" + to_string(iSt)] <= mSignalStorage["sumSS"] - mSignalStorage["z0NumS_p1_" + to_string(iSt)];
1477  }
1478  //cout<<"<<<den_p2>>>"<<endl; mSignalStorage["den_p2"].dump();
1479  //for(int iSt=0; iSt<4; iSt++) {cout<<"<<<z0NumS_"<<iSt<<">>>"<<endl; mSignalStorage["z0NumS_"+to_string(iSt)].dump();}
1480  // cotNumSZ[i] = cotNumS[i] * zxiZError[i]
1481  for (unsigned iSt = 0; iSt < 4; iSt++) {
1482  mSignalStorage["cotNumSZ_" + to_string(iSt)] <= mSignalStorage["cotNumS_" + to_string(iSt)] * mSignalStorage["zxiZError_" +
1483  to_string(iSt)];
1484  }
1485  // den = den_p2 - den_p1
1486  mSignalStorage["den"] <= mSignalStorage["den_p2"] - mSignalStorage["den_p1"];
1487  // Calculate denMax and denMin.
1488  if (mSignalStorage.find("denMin") == mSignalStorage.end()) {
1489  // Create input for calDen.
1490  double t_rr3D[4], t_wireZError[4];
1491  for (int iSt = 0; iSt < 2; iSt++) {
1492  t_rr3D[iSt] = mConstV.at("rr3D")[iSt];
1493  t_wireZError[iSt] = 1 / mConstV.at("wireZError")[iSt];
1494  }
1495  for (int iSt = 2; iSt < 4; iSt++) {
1496  t_rr3D[iSt] = mConstV.at("rr3D")[iSt];
1497  t_wireZError[iSt] = 0;
1498  }
1499  double t_denMin = Fitter3DUtility::calDenWithIZError(t_rr3D, t_wireZError);
1500  mSignalStorage["denMin"] = Belle2::TRGCDCJSignal(t_denMin, mSignalStorage["den"].getToReal(), commonData);
1501  double t_arcSMax[4], t_driftZError[4];
1502  for (unsigned iSt = 0; iSt < 4; iSt++) {
1503  t_arcSMax[iSt] = mConstD.at("Trg_PI") * mConstV.at("rr3D")[iSt] / 2;
1504  t_driftZError[iSt] = 1 / mConstV.at("driftZError")[iSt];
1505  }
1506  double t_denMax = Fitter3DUtility::calDenWithIZError(t_arcSMax, t_driftZError);
1507  mSignalStorage["denMax"] = Belle2::TRGCDCJSignal(t_denMax, mSignalStorage["den"].getToReal(), commonData);
1508  }
1509  // Constrain den.
1510  {
1511  // Make ifElse data.
1512  vector<pair<Belle2::TRGCDCJSignal, vector<pair<Belle2::TRGCDCJSignal*, Belle2::TRGCDCJSignal> > > > t_data;
1513  // Compare
1514  Belle2::TRGCDCJSignal t_compare = Belle2::TRGCDCJSignal::comp(mSignalStorage["den"], ">", mSignalStorage["denMax"]);
1515  // Assignments
1516  vector<pair<Belle2::TRGCDCJSignal*, Belle2::TRGCDCJSignal> > t_assigns = {
1517  make_pair(&mSignalStorage["den_c"], mSignalStorage["denMax"])
1518  };
1519  // Push to data.
1520  t_data.push_back(make_pair(t_compare, t_assigns));
1521  // Compare
1522  t_compare = Belle2::TRGCDCJSignal::comp(mSignalStorage["den"], ">", mSignalStorage["denMin"]);
1523  // Assignments
1524  t_assigns = {
1525  make_pair(&mSignalStorage["den_c"], mSignalStorage["den"].limit(mSignalStorage["denMin"], mSignalStorage["denMax"]))
1526  };
1527  // Push to data.
1528  t_data.push_back(make_pair(t_compare, t_assigns));
1529  // Compare
1530  t_compare = Belle2::TRGCDCJSignal();
1531  // Assignments
1532  t_assigns = {
1533  make_pair(&mSignalStorage["den_c"], mSignalStorage["denMin"])
1534  };
1535  // Push to data.
1536  t_data.push_back(make_pair(t_compare, t_assigns));
1538  }
1539  // z0NumSZ[i] = z0NumS[i]*zxiZError[i]
1540  for (unsigned iSt = 0; iSt < 4; iSt++) {
1541  mSignalStorage["z0NumSZ_" + to_string(iSt)] <= mSignalStorage["z0NumS_" + to_string(iSt)] * mSignalStorage["zxiZError_" + to_string(
1542  iSt)];
1543  }
1544  //for(int iSt=0; iSt<4; iSt++) {cout<<"<<<cotNumSZ_"<<iSt<<">>>"<<endl; mSignalStorage["cotNumSZ_"+to_string(iSt)].dump();}
1545  //cout<<"<<<den>>>"<<endl; mSignalStorage["den"].dump();
1546  //cout<<"<<<den_c>>>"<<endl; mSignalStorage["den_c"].dump();
1547  //cout<<"<<<denMin>>>"<<endl; mSignalStorage["denMin"].dump();
1548  //cout<<"<<<denMax>>>"<<endl; mSignalStorage["denMax"].dump();
1549  //cout<<"<<<offsetDen>>>"<<endl; mSignalStorage["offsetDen"].dump();
1550  //for(int iSt=0; iSt<4; iSt++) {cout<<"<<<z0NumSZ_"<<iSt<<">>>"<<endl; mSignalStorage["z0NumSZ_"+to_string(iSt)].dump();}
1551  // cot_p1[0] = cotNumSZ[0] + cotNumSZ[1]
1552  mSignalStorage["cot_p1_0"] <= mSignalStorage["cotNumSZ_0"] + mSignalStorage["cotNumSZ_1"];
1553  // cot_p1[1] = cotNumSZ[2] + cotNumSZ[3]
1554  mSignalStorage["cot_p1_1"] <= mSignalStorage["cotNumSZ_2"] + mSignalStorage["cotNumSZ_3"];
1555  // z0_p1_0 = z0NumSZ_0 + z0NumSZ_1
1556  mSignalStorage["z0_p1_0"] <= mSignalStorage["z0NumSZ_0"] + mSignalStorage["z0NumSZ_1"];
1557  // z0_p1_1 = z0NumSZ_2 + z0NumSZ_3
1558  mSignalStorage["z0_p1_1"] <= mSignalStorage["z0NumSZ_2"] + mSignalStorage["z0NumSZ_3"];
1560  //mSignalStorage["iDenMin"] <= Belle2::TRGCDCJSignal(1/t_denMax, pow(1/mSignalStorage["rho"].getToReal()/mSignalStorage["iZError2_0"].getToReal(),2));
1561  //mSignalStorage["iDenMax"] <= Belle2::TRGCDCJSignal(1/t_denMin, pow(1/mSignalStorage["rho"].getToReal()/mSignalStorage["iZError2_0"].getToReal(),2));
1562  // Calculate minInvValue, maxInvValue for LUTs
1563  if (mSignalStorage.find("invIDenMin") == mSignalStorage.end()) {
1564  unsigned long long t_int = mSignalStorage["den_c"].getMaxInt();
1565  double t_toReal = mSignalStorage["den_c"].getToReal();
1566  double t_actual = mSignalStorage["den_c"].getMaxActual();
1567  mSignalStorage["invIDenMin"] = Belle2::TRGCDCJSignal(t_int, t_toReal, t_int, t_int, t_actual, t_actual, t_actual, -1, commonData);
1568  t_int = mSignalStorage["den_c"].getMinInt();
1569  t_actual = mSignalStorage["den_c"].getMinActual();
1570  mSignalStorage["invIDenMax"] = Belle2::TRGCDCJSignal(t_int, t_toReal, t_int, t_int, t_actual, t_actual, t_actual, -1, commonData);
1571  }
1572  //cout<<"<<<invIDenMin>>>"<<endl; mSignalStorage["invIDenMin"].dump();
1573  //cout<<"<<<invIDenMax>>>"<<endl; mSignalStorage["invIDenMax"].dump();
1574  // Generate LUT(iDen = 1/den)
1575  if (mLutStorage.find("iDen") == mLutStorage.end()) {
1576  mLutStorage["iDen"] = new Belle2::TRGCDCJLUT("iDen");
1577  mLutStorage["iDen"]->setFloatFunction(
1578  [ = ](double aValue) -> double{return 1 / aValue;},
1579  mSignalStorage["den_c"],
1580  mSignalStorage["invIDenMin"], mSignalStorage["invIDenMax"],
1581  pow(1 / mSignalStorage["rho"].getToReal() / mSignalStorage["iZError2_0"].getToReal(), 2),
1582  (int)mConstD.at("iDenLUTInBitSize"), (int)mConstD.at("iDenLUTOutBitSize"));
1583  //mLutStorage["iDen"]->makeCOE("./LutData/iDen.coe");
1584  }
1585  // Operate using LUT(iDen = 1/den)
1586  mLutStorage["iDen"]->operate(mSignalStorage["den_c"], mSignalStorage["iDen"]);
1587  //for(int iSt=0; iSt<2; iSt++) {cout<<"<<<cot_p1_"<<iSt<<">>>"<<endl; mSignalStorage["cot_p1_"+to_string(iSt)].dump();}
1588  //for(int iSt=0; iSt<2; iSt++) {cout<<"<<<z0_p1_"<<iSt<<">>>"<<endl; mSignalStorage["z0_p1_"+to_string(iSt)].dump();}
1589  //cout<<"<<<offsetDen_c>>>"<<endl; mSignalStorage["offsetDen_c"].dump();
1590  //cout<<"<<<denOffset_c>>>"<<endl; mSignalStorage["denOffset_c"].dump();
1591  //cout<<"<<<iDen>>>"<<endl; mSignalStorage["iDen"].dump();
1592  // iDen = offsetIDen + iDenOffset (Already done in LUT->operate)
1593  // cot_p2 = cot_p1_0 + cot_p1_1
1594  mSignalStorage["cot_p2"] <= mSignalStorage["cot_p1_0"] + mSignalStorage["cot_p1_1"];
1595  // z0_p2 = z0_p1_0 + z0_p1_1
1596  mSignalStorage["z0_p2"] <= mSignalStorage["z0_p1_0"] + mSignalStorage["z0_p1_1"];
1597  //cout<<"<<<cot_p2>>>"<<endl; mSignalStorage["cot_p2"].dump();
1598  //cout<<"<<<z0_p2>>>"<<endl; mSignalStorage["z0_p2"].dump();
1599  // cot = cot_p2 * iDen
1600  mSignalStorage["cot"] <= mSignalStorage["cot_p2"] * mSignalStorage["iDen"];
1601  // z0 = z0_p2 * iDen
1602  mSignalStorage["z0"] <= mSignalStorage["z0_p2"] * mSignalStorage["iDen"];
1603  if (mConstD.at("JB") == 1) {cout << "<<<cot>>>" << endl; mSignalStorage["cot"].dump();}
1604  if (mConstD.at("JB") == 1) {cout << "<<<z0>>>" << endl; mSignalStorage["z0"].dump();}
1605 
1606  // Constrain z0 to [-30,30]
1607  /* cppcheck-suppress variableScope */
1608  double z0Min = -30;
1609  /* cppcheck-suppress variableScope */
1610  double z0Max = 30;
1611  // Calculate z0Max and z0Min
1612  if (mSignalStorage.find("z0Min") == mSignalStorage.end()) {
1613  mSignalStorage["z0Min"] = Belle2::TRGCDCJSignal(z0Min, mSignalStorage["z0"].getToReal(), commonData);
1614  mSignalStorage["z0Max"] = Belle2::TRGCDCJSignal(z0Max, mSignalStorage["z0"].getToReal(), commonData);
1615  }
1616  {
1617  // Make ifElse data.
1618  vector<pair<Belle2::TRGCDCJSignal, vector<pair<Belle2::TRGCDCJSignal*, Belle2::TRGCDCJSignal> > > > t_data;
1619  // Compare
1620  Belle2::TRGCDCJSignal t_compare = Belle2::TRGCDCJSignal::comp(mSignalStorage["z0"], ">", mSignalStorage["z0Max"]);
1621  // Assignments
1622  vector<pair<Belle2::TRGCDCJSignal*, Belle2::TRGCDCJSignal> > t_assigns = {
1623  make_pair(&mSignalStorage["z0_c"], mSignalStorage["z0Max"])
1624  };
1625  // Push to data.
1626  t_data.push_back(make_pair(t_compare, t_assigns));
1627  // Compare
1628  t_compare = Belle2::TRGCDCJSignal::comp(mSignalStorage["z0"], ">", mSignalStorage["z0Min"]);
1629  // Assignments
1630  t_assigns = {
1631  make_pair(&mSignalStorage["z0_c"], mSignalStorage["z0"].limit(mSignalStorage["z0Min"], mSignalStorage["z0Max"]))
1632  };
1633  // Push to data.
1634  t_data.push_back(make_pair(t_compare, t_assigns));
1635  // Compare
1636  t_compare = Belle2::TRGCDCJSignal();
1637  // Assignments
1638  t_assigns = {
1639  make_pair(&mSignalStorage["z0_c"], mSignalStorage["z0Min"])
1640  };
1641  // Push to data.
1642  t_data.push_back(make_pair(t_compare, t_assigns));
1644  }
1645  //cout<<"<<<z0Min>>>"<<endl; mSignalStorage["z0Min"].dump();
1646  //cout<<"<<<z0Max>>>"<<endl; mSignalStorage["z0Max"].dump();
1647  //cout<<"<<<z0_c>>>"<<endl; mSignalStorage["z0_c"].dump();
1648  // Constraint cot to [-0.753, 1.428]
1649  double cotMin = cos(127 * mConstD.at("Trg_PI") / 180) / sin(127 * mConstD.at("Trg_PI") / 180);
1650  double cotMax = cos(35 * mConstD.at("Trg_PI") / 180) / sin(35 * mConstD.at("Trg_PI") / 180);
1651  if (mSignalStorage.find("cotMin") == mSignalStorage.end()) {
1652  mSignalStorage["cotMin"] = Belle2::TRGCDCJSignal(cotMin, mSignalStorage["cot"].getToReal(), commonData);
1653  mSignalStorage["cotMax"] = Belle2::TRGCDCJSignal(cotMax, mSignalStorage["cot"].getToReal(), commonData);
1654  }
1655  {
1656  // Make ifElse data.
1657  vector<pair<Belle2::TRGCDCJSignal, vector<pair<Belle2::TRGCDCJSignal*, Belle2::TRGCDCJSignal> > > > t_data;
1658  // Compare
1659  Belle2::TRGCDCJSignal t_compare = Belle2::TRGCDCJSignal::comp(mSignalStorage["cot"], ">", mSignalStorage["cotMax"]);
1660  // Assignments
1661  vector<pair<Belle2::TRGCDCJSignal*, Belle2::TRGCDCJSignal> > t_assigns = {
1662  make_pair(&mSignalStorage["cot_c"], mSignalStorage["cotMax"])
1663  };
1664  // Push to data.
1665  t_data.push_back(make_pair(t_compare, t_assigns));
1666  // Compare
1667  t_compare = Belle2::TRGCDCJSignal::comp(mSignalStorage["cot"], ">", mSignalStorage["cotMin"]);
1668  // Assignments
1669  t_assigns = {
1670  make_pair(&mSignalStorage["cot_c"], mSignalStorage["cot"].limit(mSignalStorage["cotMin"], mSignalStorage["cotMax"]))
1671  };
1672  // Push to data.
1673  t_data.push_back(make_pair(t_compare, t_assigns));
1674  // Compare
1675  t_compare = Belle2::TRGCDCJSignal();
1676  // Assignments
1677  t_assigns = {
1678  make_pair(&mSignalStorage["cot_c"], mSignalStorage["cotMin"])
1679  };
1680  // Push to data.
1681  t_data.push_back(make_pair(t_compare, t_assigns));
1683  }
1684  //cout<<"<<<cotMin>>>"<<endl; mSignalStorage["cotMin"].dump();
1685  //cout<<"<<<cotMax>>>"<<endl; mSignalStorage["cotMax"].dump();
1686  //cout<<"<<<cot_c>>>"<<endl; mSignalStorage["cot_c"].dump();
1687  // Reduce number of bits for z0 to 11 bits and cot to 11 bits
1688  /* cppcheck-suppress variableScope */
1689  int z0Bits = 11;
1690  /* cppcheck-suppress variableScope */
1691  int cotBits = 11;
1692  {
1693  int z0Shift = mSignalStorage["z0_c"].getBitsize() - z0Bits;
1694  mSignalStorage["z0_r"] <= mSignalStorage["z0_c"].shift(z0Shift, 0);
1695  int cotShift = mSignalStorage["cot_c"].getBitsize() - cotBits;
1696  mSignalStorage["cot_r"] <= mSignalStorage["cot_c"].shift(cotShift, 0);
1697  }
1698  //cout<<"<<<z0_c>>>"<<endl; mSignalStorage["z0_c"].dump();
1699  //cout<<"<<<cot_c>>>"<<endl; mSignalStorage["cot_c"].dump();
1700  //cout<<"<<<z0_r>>>"<<endl; mSignalStorage["z0_r"].dump();
1701  //cout<<"<<<cot_r>>>"<<endl; mSignalStorage["cot_r"].dump();
1702 
1703 
1704  // fitDistFromZ0[i] = cot*arcS[i]
1705  for (unsigned iSt = 0; iSt < 4; iSt++) {
1706  mSignalStorage["fitDistFromZ0_" + to_string(iSt)] <= mSignalStorage["cot"] * mSignalStorage["arcS_" + to_string(iSt)];
1707  }
1708  // distFromZ0[i] = z[i] - z0
1709  for (unsigned iSt = 0; iSt < 4; iSt++) {
1710  mSignalStorage["distFromZ0_" + to_string(iSt)] <= mSignalStorage["zz_" + to_string(iSt)] - mSignalStorage["z0"];
1711  }
1712  //for(int iSt=0; iSt<4; iSt++) {cout<<"<<<fitDistFromZ0_"<<iSt<<">>>"<<endl; mSignalStorage["fitDistFromZ0_"+to_string(iSt)].dump();}
1713  //for(int iSt=0; iSt<4; iSt++) {cout<<"<<<distFromZ0_"<<iSt<<">>>"<<endl; mSignalStorage["distFromZ0_"+to_string(iSt)].dump();}
1714  // chiNum[i] = distFromZ0[i] - fitDistFromZ0[i]
1715  for (unsigned iSt = 0; iSt < 4; iSt++) {
1716  mSignalStorage["chiNum_" + to_string(iSt)] <= mSignalStorage["distFromZ0_" + to_string(iSt)] - mSignalStorage["fitDistFromZ0_" +
1717  to_string(iSt)];
1718  }
1719  //for(int iSt=0; iSt<4; iSt++) {cout<<"<<<chiNum_"<<iSt<<">>>"<<endl; mSignalStorage["chiNum_"+to_string(iSt)].dump();}
1720  // Calculate chiNumMax[i], chiNumMin[i].
1721  if (mSignalStorage.find("chiNumMax_0") == mSignalStorage.end()) {
1722  for (unsigned iSt = 0; iSt < 4; iSt++) {
1723  string t_maxName = "chiNumMax_" + to_string(iSt);
1724  string t_minName = "chiNumMin_" + to_string(iSt);
1725  string t_valueName = "chiNum_" + to_string(iSt);
1726  double t_maxValue = 2 * mConstV.at("zToOppositeStraw")[iSt];
1727  double t_minValue = 2 * mConstV.at("zToStraw")[iSt];
1728  mSignalStorage[t_maxName] = Belle2::TRGCDCJSignal(t_maxValue, mSignalStorage[t_valueName].getToReal(), commonData);
1729  mSignalStorage[t_minName] = Belle2::TRGCDCJSignal(t_minValue, mSignalStorage[t_valueName].getToReal(), commonData);
1730  }
1731  }
1732  // Constrain chiNum[i] to [-2*z_min[i], 2*z_max[i]]
1733  for (unsigned iSt = 0; iSt < 4; iSt++) {
1734  string t_maxName = "chiNumMax_" + to_string(iSt);
1735  string t_minName = "chiNumMin_" + to_string(iSt);
1736  string t_valueName = "chiNum_c_" + to_string(iSt);
1737  string t_in1Name = "chiNum_" + to_string(iSt);
1738  // Create data for ifElse
1739  vector<pair<Belle2::TRGCDCJSignal, vector<pair<Belle2::TRGCDCJSignal*, Belle2::TRGCDCJSignal> > > > t_data;
1740  // Compare
1741  Belle2::TRGCDCJSignal t_compare = Belle2::TRGCDCJSignal::comp(mSignalStorage[t_in1Name], ">", mSignalStorage[t_maxName]);
1742  // Assignments
1743  vector<pair<Belle2::TRGCDCJSignal*, Belle2::TRGCDCJSignal> > t_assigns = {
1744  make_pair(&mSignalStorage[t_valueName], mSignalStorage[t_maxName]),
1745  };
1746  // Push to data.
1747  t_data.push_back(make_pair(t_compare, t_assigns));
1748  // Compare
1749  t_compare = Belle2::TRGCDCJSignal::comp(mSignalStorage[t_in1Name], ">", mSignalStorage[t_minName]);
1750  // Assignments
1751  t_assigns = {
1752  make_pair(&mSignalStorage[t_valueName], mSignalStorage[t_in1Name].limit(mSignalStorage[t_minName], mSignalStorage[t_maxName])),
1753  };
1754  // Push to data.
1755  t_data.push_back(make_pair(t_compare, t_assigns));
1756  // Compare
1757  t_compare = Belle2::TRGCDCJSignal();
1758  // Assignments
1759  t_assigns = {
1760  make_pair(&mSignalStorage[t_valueName], mSignalStorage[t_minName]),
1761  };
1762  // Push to data.
1763  t_data.push_back(make_pair(t_compare, t_assigns));
1764  // Process ifElse data.
1766  }
1767  //for(int iSt=0; iSt<4; iSt++) {cout<<"<<<chiNumMax_"<<iSt<<">>>"<<endl; mSignalStorage["chiNumMax_"+to_string(iSt)].dump();}
1768  //for(int iSt=0; iSt<4; iSt++) {cout<<"<<<chiNumMin_"<<iSt<<">>>"<<endl; mSignalStorage["chiNumMin_"+to_string(iSt)].dump();}
1769  //for(int iSt=0; iSt<4; iSt++) {cout<<"<<<chiNum_c_"<<iSt<<">>>"<<endl; mSignalStorage["chiNum_c_"+to_string(iSt)].dump();}
1770  // chi2_p1[i] = chiNum_c[i] * chiNum_c[i]
1771  for (unsigned iSt = 0; iSt < 4; iSt++) {
1772  mSignalStorage["chi2_p1_" + to_string(iSt)] <= mSignalStorage["chiNum_c_" + to_string(iSt)] * mSignalStorage["chiNum_c_" +
1773  to_string(iSt)];
1774  }
1775  //for(int iSt=0; iSt<4; iSt++) {cout<<"<<<chi2_p1_"<<iSt<<">>>"<<endl; mSignalStorage["chi2_p1_"+to_string(iSt)].dump();}
1776  // chi2[i] = chi2_p1[i] * iError2[i]
1777  for (unsigned iSt = 0; iSt < 4; iSt++) {
1778  mSignalStorage["chi2_" + to_string(iSt)] <= mSignalStorage["chi2_p1_" + to_string(iSt)] * mSignalStorage["iZError2_" + to_string(
1779  iSt)];
1780  }
1781  //for(int iSt=0; iSt<4; iSt++) {cout<<"<<<chi2_"<<iSt<<">>>"<<endl; mSignalStorage["chi2_"+to_string(iSt)].dump();}
1782  // chi2Sum_p1_0 = chi2_0 + chi2_1
1783  mSignalStorage["chi2Sum_p1_0"] <= mSignalStorage["chi2_0"] + mSignalStorage["chi2_1"];
1784  // chi2Sum_p1_1 = chi2_2 + chi2_3
1785  mSignalStorage["chi2Sum_p1_1"] <= mSignalStorage["chi2_2"] + mSignalStorage["chi2_3"];
1786  //cout<<"<<<chi2Sum_p1_0>>>"<<endl; mSignalStorage["chi2Sum_p1_0"].dump();
1787  //cout<<"<<<chi2Sum_p1_1>>>"<<endl; mSignalStorage["chi2Sum_p1_1"].dump();
1788  // chi2Sum = chi2Sum_p1_0 + chi2Sum_p1_1
1789  mSignalStorage["zChi2"] <= (mSignalStorage["chi2Sum_p1_0"] + mSignalStorage["chi2Sum_p1_1"]).shift(1);
1790  if (mConstD.at("JB") == 1) {cout << "<<<zChi2>>>" << endl; mSignalStorage["zChi2"].dump();}
1791 
1792  // Constrain zChi2 to [0, 100]
1793  /* cppcheck-suppress variableScope */
1794  double zChi2Min = 0;
1795  /* cppcheck-suppress variableScope */
1796  double zChi2Max = 100;
1797  /* cppcheck-suppress variableScope */
1798  int zChi2Bits = 4;
1799  // Calculate zChi2Max and zChi2Min
1800  if (mSignalStorage.find("zChi2Min") == mSignalStorage.end()) {
1801  mSignalStorage["zChi2Min"] = Belle2::TRGCDCJSignal(zChi2Min, mSignalStorage["zChi2"].getToReal(), commonData);
1802  mSignalStorage["zChi2Max"] = Belle2::TRGCDCJSignal(zChi2Max, mSignalStorage["zChi2"].getToReal(), commonData);
1803  }
1804  {
1805  // Make ifElse data.
1806  vector<pair<Belle2::TRGCDCJSignal, vector<pair<Belle2::TRGCDCJSignal*, Belle2::TRGCDCJSignal> > > > t_data;
1807  // Compare
1808  Belle2::TRGCDCJSignal t_compare = Belle2::TRGCDCJSignal::comp(mSignalStorage["zChi2"], ">", mSignalStorage["zChi2Max"]);
1809  // Assignments
1810  vector<pair<Belle2::TRGCDCJSignal*, Belle2::TRGCDCJSignal> > t_assigns = {
1811  make_pair(&mSignalStorage["zChi2_c"], mSignalStorage["zChi2Max"])
1812  };
1813  // Push to data.
1814  t_data.push_back(make_pair(t_compare, t_assigns));
1815  // Compare
1816  t_compare = Belle2::TRGCDCJSignal::comp(mSignalStorage["zChi2"], ">", mSignalStorage["zChi2Min"]);
1817  // Assignments
1818  t_assigns = {
1819  make_pair(&mSignalStorage["zChi2_c"], mSignalStorage["zChi2"].limit(mSignalStorage["zChi2Min"], mSignalStorage["zChi2Max"]))
1820  };
1821  // Push to data.
1822  t_data.push_back(make_pair(t_compare, t_assigns));
1823  // Compare
1824  t_compare = Belle2::TRGCDCJSignal();
1825  // Assignments
1826  t_assigns = {
1827  make_pair(&mSignalStorage["zChi2_c"], mSignalStorage["zChi2Min"])
1828  };
1829  // Push to data.
1830  t_data.push_back(make_pair(t_compare, t_assigns));
1832  }
1833  //cout<<"<<<zChi2Min>>>"<<endl; mSignalStorage["zChi2Min"].dump();
1834  //cout<<"<<<zChi2Max>>>"<<endl; mSignalStorage["zChi2Max"].dump();
1835  //cout<<"<<<zChi2_c>>>"<<endl; mSignalStorage["zChi2_c"].dump();
1836  {
1837  int zChi2Shift = mSignalStorage["zChi2_c"].getBitsize() - zChi2Bits;
1838  mSignalStorage["zChi2_r"] <= mSignalStorage["zChi2_c"].shift(zChi2Shift, 0);
1839  }
1840  //cout<<"<<<zChi2_c>>>"<<endl; mSignalStorage["zChi2_c"].dump();
1841  //cout<<"<<<zChi2_r>>>"<<endl; mSignalStorage["zChi2_r"].dump();
1842 
1843 }
1844 
1845 void Fitter3DUtility::fitter3D(std::map<std::string, std::vector<double> >& stGeometry,
1846  std::vector<std::vector<double> > const& stXts,
1847  int eventTimeValid, int eventTime,
1848  std::vector<std::vector<int> > const& rawStTSs,
1849  int charge, double radius, double phi_c, double& z0, double& cot, double& chi2)
1850 {
1851  vector<double> zz;
1852  vector<double> arcS;
1853  vector<double> invZError2;
1854  fitter3D(stGeometry, stXts, eventTimeValid, eventTime, rawStTSs, charge, radius, phi_c, z0, cot, chi2, arcS, zz, invZError2);
1855 }
1856 
1857 void Fitter3DUtility::fitter3D(std::map<std::string, std::vector<double> >& stGeometry,
1858  std::vector<std::vector<double> > const& stXts,
1859  int eventTimeValid, int eventTime,
1860  std::vector<std::vector<int> > const& rawStTSs,
1861  int charge, double radius, double phi_c, double& z0, double& cot, double& chi2, vector<double>& arcS, vector<double>& zz,
1862  vector<double>& invZError2)
1863 {
1864  vector<double> stTSs(4);
1865  Fitter3DUtility::calPhiFast(stGeometry, stXts, eventTimeValid, eventTime, rawStTSs, stTSs);
1866  Fitter3DUtility::setErrorFast(rawStTSs, eventTimeValid, invZError2);
1867  zz = vector<double> (4, 0);
1868  arcS = vector<double> (4, 0);
1869  for (unsigned iSt = 0; iSt < 4; iSt++) {
1870  if (rawStTSs[iSt][1] != 0) {
1871  zz[iSt] = Fitter3DUtility::calZ(charge, stGeometry["angleSt"][iSt], stGeometry["zToStraw"][iSt],
1872  stGeometry["cdcRadius"][iSt], stTSs[iSt], radius, phi_c);
1873  arcS[iSt] = Fitter3DUtility::calS(radius, stGeometry["cdcRadius"][iSt]);
1874  }
1875  }
1876  Fitter3DUtility::rSFit(&invZError2[0], &arcS[0], &zz[0], z0, cot, chi2);
1877 }
1878 
1879 void Fitter3DUtility::fitter3DFirm(std::map<std::string, double>& mConstD, std::map<std::string, std::vector<double> >& mConstV,
1880  int eventTimeValid, int eventTime,
1881  std::vector<std::vector<int> > const& rawStTSs,
1882  int charge, double radius, double phi_c,
1883  Belle2::TRGCDCJSignalData* commonData, std::map<std::string, Belle2::TRGCDCJSignal>& mSignalStorage,
1884  std::map<std::string, Belle2::TRGCDCJLUT*>& mLutStorage)
1885 {
1886  // Change to Signals.
1887  // rawStTSs[iSt] = [TS ID, TS LR, TS driftTime]
1888  vector<tuple<string, double, int, double, double, int> > t_values = {
1889  make_tuple("phi0", phi_c, mConstD["phiBitSize"], mConstD["phiMin"], mConstD["phiMax"], 0),
1890  make_tuple("rho", radius, mConstD["rhoBitSize"], mConstD["rhoMin"], mConstD["rhoMax"], 0),
1891  make_tuple("charge", (int)(charge == 1 ? 1 : 0), 1, 0, 1.5, 0),
1892  make_tuple("lr_0", rawStTSs[0][1], 2, 0, 3.5, 0),
1893  make_tuple("lr_1", rawStTSs[1][1], 2, 0, 3.5, 0),
1894  make_tuple("lr_2", rawStTSs[2][1], 2, 0, 3.5, 0),
1895  make_tuple("lr_3", rawStTSs[3][1], 2, 0, 3.5, 0),
1896  make_tuple("tsId_0", rawStTSs[0][0], ceil(log(mConstV["nTSs"][1]) / log(2)), 0, pow(2, ceil(log(mConstV["nTSs"][1]) / log(2))) - 0.5, 0),
1897  make_tuple("tsId_1", rawStTSs[1][0], ceil(log(mConstV["nTSs"][3]) / log(2)), 0, pow(2, ceil(log(mConstV["nTSs"][3]) / log(2))) - 0.5, 0),
1898  make_tuple("tsId_2", rawStTSs[2][0], ceil(log(mConstV["nTSs"][5]) / log(2)), 0, pow(2, ceil(log(mConstV["nTSs"][5]) / log(2))) - 0.5, 0),
1899  make_tuple("tsId_3", rawStTSs[3][0], ceil(log(mConstV["nTSs"][7]) / log(2)), 0, pow(2, ceil(log(mConstV["nTSs"][7]) / log(2))) - 0.5, 0),
1900  make_tuple("tdc_0", rawStTSs[0][2], mConstD["tdcBitSize"], 0, pow(2, mConstD["tdcBitSize"]) - 0.5, 0),
1901  make_tuple("tdc_1", rawStTSs[1][2], mConstD["tdcBitSize"], 0, pow(2, mConstD["tdcBitSize"]) - 0.5, 0),
1902  make_tuple("tdc_2", rawStTSs[2][2], mConstD["tdcBitSize"], 0, pow(2, mConstD["tdcBitSize"]) - 0.5, 0),
1903  make_tuple("tdc_3", rawStTSs[3][2], mConstD["tdcBitSize"], 0, pow(2, mConstD["tdcBitSize"]) - 0.5, 0),
1904  make_tuple("eventTime", eventTime, mConstD["tdcBitSize"], 0, pow(2, mConstD["tdcBitSize"]) - 0.5, 0),
1905  make_tuple("eventTimeValid", eventTimeValid, 1, 0, 1.5, 0),
1906  };
1907  Belle2::TRGCDCJSignal::valuesToMapSignals(t_values, commonData, mSignalStorage);
1908  // Calculate complex fit3D
1909  Fitter3DUtility::setError(mConstD, mConstV, mSignalStorage);
1910  Fitter3DUtility::calPhi(mConstD, mConstV, mSignalStorage, mLutStorage);
1911  Fitter3DUtility::constrainRPerStSl(mConstV, mSignalStorage);
1912  Fitter3DUtility::calZ(mConstD, mConstV, mSignalStorage, mLutStorage);
1913  Fitter3DUtility::calS(mConstD, mConstV, mSignalStorage, mLutStorage);
1914  Fitter3DUtility::rSFit(mConstD, mConstV, mSignalStorage, mLutStorage);
1915  // Post handling of signals.
1916  // Name all signals.
1917  if ((*mSignalStorage.begin()).second.getName() == "") {
1918  for (auto it = mSignalStorage.begin(); it != mSignalStorage.end(); ++it) {
1919  (*it).second.setName((*it).first);
1920  }
1921  }
1922 }
1923 
1924 void Fitter3DUtility::findImpactPosition(TVector3* mcPosition, TLorentzVector* mcMomentum, int charge, TVector2& helixCenter,
1925  TVector3& impactPosition)
1926 {
1927 
1928  // Finds the impact position. Everything is in cm, and GeV.
1929  // Input: production vertex (mcx, mcy, mcz),
1930  // momentum at production vertex (px, py, pz)
1931  // charge of particle.
1932  // Output: helix center's coordiante (hcx, hcy)
1933  // impact position (impactX, impactY, impactZ)
1934 
1935  double rho = sqrt(pow(mcMomentum->Px(), 2) + pow(mcMomentum->Py(), 2)) / 0.3 / 1.5 * 100;
1936  double hcx = mcPosition->X() + rho * cos(atan2(mcMomentum->Py(), mcMomentum->Px()) - charge * TMath::Pi() / 2);
1937  double hcy = mcPosition->Y() + rho * sin(atan2(mcMomentum->Py(), mcMomentum->Px()) - charge * TMath::Pi() / 2);
1938  helixCenter.Set(hcx, hcy);
1939  double impactX = (helixCenter.Mod() - rho) / helixCenter.Mod() * helixCenter.X();
1940  double impactY = (helixCenter.Mod() - rho) / helixCenter.Mod() * helixCenter.Y();
1941  int signdS;
1942  if (atan2(impactY, impactX) < atan2(mcPosition->Y(), mcPosition->X())) signdS = -1;
1943  else signdS = 1;
1944  double dS = 2 * rho * asin(sqrt(pow(impactX - mcPosition->X(), 2) + pow(impactY - mcPosition->Y(), 2)) / 2 / rho);
1945  double impactZ = mcMomentum->Pz() / mcMomentum->Pt() * dS * signdS + mcPosition->Z();
1946  impactPosition.SetXYZ(impactX, impactY, impactZ);
1947 
1948 }
1949 
1950 void Fitter3DUtility::calHelixParameters(TVector3 position, TVector3 momentum, int charge, TVectorD& helixParameters)
1951 {
1952  double Trg_PI = 3.141592653589793;
1953  // HelixParameters: dR, phi0, keppa, dz, tanLambda
1954  double t_alpha = 1 / 0.3 / 1.5 * 100;
1955  double t_pT = momentum.Perp();
1956  double t_R = t_pT * t_alpha;
1957  helixParameters.Clear();
1958  helixParameters.ResizeTo(5);
1959  helixParameters[2] = t_alpha / t_R;
1960  helixParameters[1] = atan2(position.Y() - t_R * momentum.X() / t_pT * charge, position.X() + t_R * momentum.Y() / t_pT * charge);
1961  helixParameters[0] = (position.X() + t_R * momentum.Y() / t_pT * charge) / cos(helixParameters[1]) - t_R;
1962  double t_phi = atan2(-momentum.X() * charge, momentum.Y() * charge) - helixParameters[1];
1963  if (t_phi > Trg_PI) t_phi -= 2 * Trg_PI;
1964  if (t_phi < -Trg_PI) t_phi += 2 * Trg_PI;
1965  helixParameters[4] = momentum.Z() / t_pT * charge;
1966  helixParameters[3] = position.Z() + helixParameters[4] * t_R * t_phi;
1967 }
1968 void Fitter3DUtility::calVectorsAtR(TVectorD& helixParameters, int charge, double cdcRadius, TVector3& position, TVector3& momentum)
1969 {
1970  // HelixParameters: dR, phi0, keppa, dz, tanLambda
1971  double t_alpha = 1 / 0.3 / 1.5 * 100;
1972  double t_R = t_alpha / helixParameters[2];
1973  double t_pT = t_R / t_alpha;
1974  double t_phi = -1 * charge * acos((-pow(cdcRadius, 2) + pow(t_R + helixParameters[0], 2) + pow(t_R,
1975  2)) / (2 * t_R * (t_R + helixParameters[0])));
1976  double t_X = (helixParameters[0] + t_R) * cos(helixParameters[1]) - t_R * cos(helixParameters[1] + t_phi);
1977  double t_Y = (helixParameters[0] + t_R) * sin(helixParameters[1]) - t_R * sin(helixParameters[1] + t_phi);
1978  double t_Z = helixParameters[3] - helixParameters[4] * t_R * t_phi;
1979  double t_Px = -t_pT * sin(helixParameters[1] + t_phi) * charge;
1980  double t_Py = t_pT * cos(helixParameters[1] + t_phi) * charge;
1981  double t_Pz = t_pT * helixParameters[4] * charge;
1982  position.SetXYZ(t_X, t_Y, t_Z);
1983  momentum.SetXYZ(t_Px, t_Py, t_Pz);
1984 }
1985 
1986 int Fitter3DUtility::bitSize(int numberBits, int mode)
1987 {
1988  int bitsize = 1;
1989  if (mode == 1) {
1990  for (int i = 0; i < numberBits - 1; i++) {
1991  bitsize *= 2;
1992  }
1993  bitsize -= 1;
1994  } else if (mode == 0) {
1995  for (int i = 0; i < numberBits; i++) {
1996  bitsize *= 2;
1997  }
1998  }
1999  return bitsize;
2000 }
2001 
2002 void Fitter3DUtility::changeInteger(int& integer, double real, double minValue, double maxValue, int bitSize)
2003 {
2004  // Version 1.
2005  //double range = maxValue - minValue;
2006  //double convert = bitSize/range;
2008  //integer = int( (real - minValue) * convert +0.5);
2009  // Version 2.
2010  double range = maxValue - minValue;
2011  double convert = (bitSize - 0.5) / range;
2012  integer = FpgaUtility::roundInt((real - minValue) * convert);
2013 }
2014 
2015 void Fitter3DUtility::changeReal(double& real, int integer, double minValue, double maxValue, int bitSize)
2016 {
2018  //double range = maxValue - minValue;
2019  //double convert = bitSize/range;
2020  //real = (integer/convert) + minValue;
2021  // Version 2.
2022  double range = maxValue - minValue;
2023  double convert = (bitSize - 0.5) / range;
2024  real = (integer / convert) + minValue;
2025 }
2026 
2027 
2028 void Fitter3DUtility::findExtreme(double& m_max, double& m_min, double value)
2029 {
2030  if (value > m_max) m_max = value;
2031  if (value < m_min) m_min = value;
2032 }
Fitter3DUtility::fitter3DFirm
static void fitter3DFirm(std::map< std::string, double > &mConstD, std::map< std::string, std::vector< double > > &mConstV, int eventTimeValid, int eventTime, std::vector< std::vector< int > > const &rawStTSs, int charge, double radius, double phi_c, Belle2::TRGCDCJSignalData *commonData, std::map< std::string, Belle2::TRGCDCJSignal > &mSignalStorage, std::map< std::string, Belle2::TRGCDCJLUT * > &mLutStorage)
Combines several functions for fitter3D firmware.
Definition: Fitter3DUtility.cc:1879
FpgaUtility::roundInt
static double roundInt(double value)
Round double value.
Definition: FpgaUtility.cc:31
Fitter3DUtility::findImpactPosition
static void findImpactPosition(TVector3 *mcPosition, TLorentzVector *mcMomentum, int charge, TVector2 &helixCenter, TVector3 &impactPosition)
MC calculation functions Calculates the impact position of track.
Definition: Fitter3DUtility.cc:1924
Fitter3DUtility::fitter3D
static void fitter3D(std::map< std::string, std::vector< double > > &stGeometry, std::vector< std::vector< double > > const &stXts, int eventTimeValid, int eventTime, std::vector< std::vector< int > > const &rawStTSs, int charge, double radius, double phi_c, double &z0, double &cot, double &chi2)
Combines several functions for fitter3D.
Definition: Fitter3DUtility.cc:1845
Fitter3DUtility::toUnsignedTdc
static unsigned toUnsignedTdc(int tdc, int nBits)
Changes tdc and event time to unsigned value that has # bits.
Definition: Fitter3DUtility.cc:226
Fitter3DUtility::calDeltaPhi
static double calDeltaPhi(int charge, double anglest, double ztostraw, double rr, double phi, double rho, double phi0)
Calculates the phi difference between fitted axial phi and stereo phi.
Definition: Fitter3DUtility.cc:808
Fitter3DUtility::calVectorsAtR
static void calVectorsAtR(TVectorD &helixParameters, int charge, double radius, TVector3 &position, TVector3 &momentum)
Calculates position and momentum at a certain radius.
Definition: Fitter3DUtility.cc:1968
Fitter3DUtility::changeInteger
static void changeInteger(int &integer, double real, double minValue, double maxValue, int bitSize)
Changes float to integer value.
Definition: Fitter3DUtility.cc:2002
Fitter3DUtility::findExtreme
static void findExtreme(double &m_max, double &m_min, double value)
Finds maximum and minium values.
Definition: Fitter3DUtility.cc:2028
Fitter3DUtility::calDen
static double calDen(double *arcS, double *zError)
Calculates the denominator for fitting z and arc s.
Definition: Fitter3DUtility.cc:1281
Belle2::TRGCDCJSignal
A class to use Signals for TRGCDC 3D tracker.
Definition: JSignal.h:36
Belle2::TRGCDCJSignal::comp
static TRGCDCJSignal comp(TRGCDCJSignal const &lhs, std::string operate, TRGCDCJSignal const &rhs)
Compare two signals.
Definition: JSignal.cc:1173
Fitter3DUtility::rPhiFit2
static void rPhiFit2(double *rr, double *phi2, double *phierror, double &rho, double &myphi0, int nTS)
A circle fitter.
Definition: Fitter3DUtility.cc:178
Fitter3DUtility::setError
static void setError(std::map< std::string, double > const &mConstD, std::map< std::string, std::vector< double > > const &mConstV, std::map< std::string, Belle2::TRGCDCJSignal > &mSignalStorage)
Sets error using JSignal class.
Definition: Fitter3DUtility.cc:683
Fitter3DUtility::calZ
static double calZ(int charge, double anglest, double ztostraw, double rr, double phi, double rho, double phi0)
Calculates z.
Definition: Fitter3DUtility.cc:896
Belle2::TRGCDCJSignalData
A class to hold common data for JSignals.
Definition: JSignalData.h:34
Fitter3DUtility::rotateTsId
static int rotateTsId(int value, int refId, int nTSs)
Rotates to range [0, nTSs-1].
Definition: Fitter3DUtility.cc:630
Fitter3DUtility::rPhiFitter
static void rPhiFitter(double *rr, double *phi2, double *invphierror, double &rho, double &myphi0)
A circle fitter with invPhiError without fit chi2 output.
Definition: Fitter3DUtility.cc:69
Fitter3DUtility::calPhi
static double calPhi(double wirePhi, double driftLength, double rr, int lr)
Pre 3D fitter functions. rr is in cm scale. driftLength is in cm scale.
Definition: Fitter3DUtility.cc:239
Fitter3DUtility::bitSize
static int bitSize(int numberBits, int mode)
Firmware convert functions.
Definition: Fitter3DUtility.cc:1986
Fitter3DUtility::calS
static double calS(double rho, double rr)
Calculates arc length.
Definition: Fitter3DUtility.cc:1209
Fitter3DUtility::constrainRPerStSl
static void constrainRPerStSl(std::map< std::string, std::vector< double > > const &mConstV, std::map< std::string, Belle2::TRGCDCJSignal > &mSignalStorage)
Constrains R for each SL differently using JSignal and multiple constants.
Definition: Fitter3DUtility.cc:829
Fitter3DUtility::calDenWithIZError
static double calDenWithIZError(double *arcS, double *iZError)
Calculates the denominator for fitting z and arc s.
Definition: Fitter3DUtility.cc:1294
Fitter3DUtility::stereoIdToPhi
static double stereoIdToPhi(std::vector< double > &stNWires, int iSt, int id)
Converts stereo ts id to phi.
Definition: Fitter3DUtility.cc:576
Fitter3DUtility::changeReal
static void changeReal(double &real, int integer, double minValue, double maxValue, int bitSize)
Changes integer to float value.
Definition: Fitter3DUtility.cc:2015
Belle2::TRGCDCJLUT
A class to use LUTs for TRGCDC.
Definition: JLUT.h:36
Fitter3DUtility::setErrorFast
static void setErrorFast(std::vector< std::vector< int > > const &rawStTSs, int eventTimeValid, std::vector< double > &invZError2)
Sets error for fitting.
Definition: Fitter3DUtility.cc:665
Belle2::TRGCDCJSignal::ifElse
static void ifElse(std::vector< std::pair< TRGCDCJSignal, std::vector< std::pair< TRGCDCJSignal *, TRGCDCJSignal > > > > &data, int targetClock)
If else implementation with target clock.
Definition: JSignal.cc:804
Belle2::TRGCDCJSignal::valuesToMapSignals
static void valuesToMapSignals(std::vector< std::tuple< std::string, double, int, double, double, int > > const &inValues, Belle2::TRGCDCJSignalData *inCommonData, std::map< std::string, Belle2::TRGCDCJSignal > &outMap)
Values => [name, value, bitwidth, min, max, clock] Changes values to signals.
Definition: JSignal.cc:2207
Fitter3DUtility::calPhiFast
static void calPhiFast(std::map< std::string, std::vector< double > > &stGeometry, std::vector< std::vector< double > > const &stXts, int eventTimeValid, int eventTime, std::vector< std::vector< int > > const &rawStTSs, std::vector< double > &stTSs)
Calculates phi with fast simulation.
Definition: Fitter3DUtility.cc:583
Fitter3DUtility::rSFit2
static void rSFit2(double *iezz21, double *iezz22, double *arcS, double *zz, int *lutv, double &z0, double &cot, double &zchi2)
Fits z and arc S. (Will be deprecated.)
Definition: Fitter3DUtility.cc:1348
Fitter3DUtility::findQuadrant
static int findQuadrant(double value)
Finds quadrant of angle. Angle is in rad.
Definition: Fitter3DUtility.cc:642
Belle2::TRGCDCJSignal::absolute
static const TRGCDCJSignal absolute(TRGCDCJSignal const &first)
Absolute TRGCDCJSignal. Removes 1 bit if signed or minus unsigned.
Definition: JSignal.cc:1566
Fitter3DUtility::rPhiFit
static void rPhiFit(double *rr, double *phi2, double *phierror, double &rho, double &myphi0)
A circle fitter.
Definition: Fitter3DUtility.cc:57
Fitter3DUtility::saveStereoXt
static void saveStereoXt(std::vector< std::vector< double > > &stXts, std::string const &filePrefix)
Saves stereo Xt to file.
Definition: Fitter3DUtility.cc:543
Fitter3DUtility::chargeFinder
static void chargeFinder(double *nTSs, double *tsIds, double *tsHitmap, double phi0, double inCharge, double &outCharge)
Charge finder using circle fitter output and axial TSs.
Definition: Fitter3DUtility.cc:140
Fitter3DUtility::rSFit
static void rSFit(double *iezz2, double *arcS, double *zz, double &z0, double &cot, double &zchi2)
3D fitter functions Fits z and arc S.
Definition: Fitter3DUtility.cc:1308
Fitter3DUtility::calHelixParameters
static void calHelixParameters(TVector3 position, TVector3 momentum, int charge, TVectorD &helixParameters)
HelixParameters: dR, phi0, keppa, dz, tanLambda Calculates the helix parameters of track.
Definition: Fitter3DUtility.cc:1950
Fitter3DUtility::calStAxPhi
static double calStAxPhi(int charge, double anglest, double ztostraw, double rr, double rho, double phi0)
Calculates the fitted axial phi for the stereo super layer.
Definition: Fitter3DUtility.cc:787
Fitter3DUtility::findSign
static int findSign(double *phi2)
2D fitter functions
Definition: Fitter3DUtility.cc:36
Fitter3DUtility::loadStereoXt
static void loadStereoXt(std::string const &filePrefix, int nFiles, std::vector< std::vector< double > > &stXts)
Load stereo Xt file.
Definition: Fitter3DUtility.cc:557
Fitter3DUtility::rotatePhi
static double rotatePhi(double value, double refPhi)
Rotates to range [-pi, pi].
Definition: Fitter3DUtility.cc:609