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