/*
  phi = phi00 + charge*fStep*sin(theta)/RH;
  projpos[0] = trackout[0] + charge*RH *(sin(phi) - sin(phi00));
  projpos[1] = trackout[1] - charge*RH *(cos(phi) - cos(phi00));
  projpos[2] = trackout[2] + charge*RH *(phi - phi00)/tan(theta);

*/

#include <iostream>
#include <fstream>
#include <iomanip>
#include <vector>
#include "TH1.h"
#include "TH1F.h"
#include "TH2F.h"
#include "TProfile.h"
#include "TTree.h"
#include "TFile.h"
#include "TStyle.h"
#include "TCanvas.h"
#include "TMath.h"
#include "TMinuit.h"
#include "TObject.h"
#include "TPostScript.h"
#include <TRandom3.h>
#include "CLHEP/Vector/ThreeVector.h"
#include "CLHEP/Vector/LorentzVector.h"
#include "CLHEP/Matrix/Matrix.h"

#define SMEARING

using namespace std;
using namespace CLHEP;

const double pival = acos(-1.0);
const double twopi = 2*pival;
const double pibytwo = pival/2.;
const double pibyfour = pival/4.;
const double radtodeg = 180./pival;

const int nNoiseHit=20; //Constant noise hit in the detector;
const int nsilayer=13;
double sirad[nsilayer]={0.029, 0.068, 0.109, 0.160, 0.220, 0.290, 0.370, 0.450, 0.520, 0.590, 0.660, 0.730, 0.800}; 
//const int nradius = nsilayer + 1;

//convert digisignal to hit position (3vector) including inefficinecy and noise 
void fill_digipos_array(int nhit, unsigned int* detid, float* simenr, float* simtime, vector<Hep3Vector>& hitpos);

TRandom3* gRandom3 = new TRandom3();
int main() {
  gStyle->SetOptStat(111111);

  unsigned   irun;                // Run number of these events
  unsigned   ievt;                //Event number
  unsigned   ngent;
  float	ievt_wt;		//

  const unsigned int ngenmx=50;
  int   pidin[ngenmx]; 	  //PID of incident particle
  float momin[ngenmx]; 	  //Energy of incident particle
  float thein[ngenmx];	  //Initial polar angle of incident particle
  float phiin[ngenmx];     //Initial azimuthal angle of incident particle 
  float posxin[ngenmx];	  //Initial X-position
  float posyin[ngenmx];     //Initial Y-position
  float poszin[ngenmx];     //Initial Z-position

  unsigned int nsimhtTk;
  const unsigned int nsimhtmxTk=2000;
  unsigned int detidTk[nsimhtmxTk];

  float simtimeTk[nsimhtmxTk];
  float simenrTk[nsimhtmxTk];

  int   simpdgidTk[nsimhtmxTk];
  float simvxTk[nsimhtmxTk]; 
  float simvyTk[nsimhtmxTk];
  float simvzTk[nsimhtmxTk];
  float simpxTk[nsimhtmxTk];
  float simpyTk[nsimhtmxTk];
  float simpzTk[nsimhtmxTk];

  float simloctheTk[nsimhtmxTk];
  float simlocphiTk[nsimhtmxTk];

  float fittermom;

  //Store to all hists in Hep3vector
  vector<Hep3Vector> digihitpos;
  //  cout<<"1mean "<<endl;
  TFile* fileOut = new TFile("output_merge_muon.root", "recreate");  
  TTree* Tout = new TTree("T2", "test");;

  Tout->Branch("irun",&irun,"irun/i");
  Tout->Branch("ievt",&ievt,"ievt/i");
  
  Tout->Branch("ngent",&ngent,"ngent/i");
  Tout->Branch("pidin",pidin,"pidin[ngent]/I");
  Tout->Branch("ievt_wt",&ievt_wt,"ievt_wt/F"); //Generated events with different weight
  Tout->Branch("momin",momin,"momin[ngent]/F");
  Tout->Branch("thein",thein,"thein[ngent]/F");
  Tout->Branch("phiin",phiin,"phiin[ngent]/F");
  Tout->Branch("posxin",posxin,"posxin[ngent]/F");
  Tout->Branch("posyin",posyin,"posyin[ngent]/F");
  Tout->Branch("poszin",poszin,"poszin[ngent]/F");

  Tout->Branch("fittermom",&fittermom,"fittermom/F");

  TH1F* h_momentum = new TH1F("hgen_mom", "gen_mom (GeV)", 120, -100, 100.0);
  TH1F* h_theta = new TH1F("hgen_theta", "gen_theta (rad)", 120, -pibyfour, -pibyfour);
  TH1F* h_phi = new TH1F("hgen_phi", "gen_phi (rad)", 120, -pibytwo, pibytwo);
  TH1F* simEnergy = new TH1F("simEnergy", "Energy in Si layer (KeV)", 1200, 0.0, 1200.0);

  //Input files
 
  TFile* fileIn = new TFile("test_muon_run10.root", "read");
  TTree *Tin = (TTree*)fileIn->Get("T1");
  
  Tin->SetBranchAddress("irun",&irun);
  Tin->SetBranchAddress("ievt",&ievt);
  
  Tin->SetBranchAddress("ngent",&ngent);
  Tin->SetBranchAddress("pidin",pidin);
  Tin->SetBranchAddress("ievt_wt",&ievt_wt);
  Tin->SetBranchAddress("momin",momin);
  Tin->SetBranchAddress("thein",thein);
  Tin->SetBranchAddress("phiin",phiin);
  Tin->SetBranchAddress("posxin",posxin);
  Tin->SetBranchAddress("posyin",posyin);
  Tin->SetBranchAddress("poszin",poszin);
  
  //SImulated output of tracker
  Tin->SetBranchAddress("nsimhtTk", &nsimhtTk);
  Tin->SetBranchAddress("detidTk", detidTk);
  Tin->SetBranchAddress("simpdgidTk", simpdgidTk);
  Tin->SetBranchAddress("simtimeTk", simtimeTk);
  Tin->SetBranchAddress("simenrTk", simenrTk);
  Tin->SetBranchAddress("simvxTk", simvxTk);
  Tin->SetBranchAddress("simvyTk", simvyTk);
  Tin->SetBranchAddress("simvzTk", simvzTk);
  Tin->SetBranchAddress("simpxTk", simpxTk);
  Tin->SetBranchAddress("simpyTk", simpyTk);
  Tin->SetBranchAddress("simpzTk", simpzTk);
  
  int nentries = Tin->GetEntries();
  cout <<"nentr "<<nentries<<endl;;
  const int nentrymx=100;
  for (int iev=0; iev<min(nentries, nentrymx); iev++) {
    fileIn->cd();
    Tin->GetEntry(iev); //+96427); //(+1853); // (1715);
    
    fileOut->cd();
    h_momentum->Fill(momin[0]/1000.0);
    h_theta->Fill(thein[0]);
    h_phi->Fill(phiin[0]);

    fittermom = momin[0];
    digihitpos.clear();
    //      cout<<" SimHit Size "<<nsimhtTk<<" "<< momin[0]<<" "<<pidin[0]<<" "<<digihitpos.size()<<endl;
    //Decode rad/theta/phi and also add efficiency and noise
    if (simenrTk==0) continue;
    for (int ix=0; ix<nsimhtTk; ix++) {
      simEnergy->Fill(simenrTk[ix]);
    }
    
    fill_digipos_array(nsimhtTk, detidTk, simenrTk, simtimeTk, digihitpos);
    for (int ix=0; ix<digihitpos.size(); ix++) {
      cout <<iev<<" "<<ix<< digihitpos[ix]<<" "<<digihitpos[ix].x()<<" "<<digihitpos[ix].y()<<" "<<digihitpos[ix].z()<<" "<<digihitpos[ix].rho()<<" "<<digihitpos[ix].phi()<<digihitpos[ix].theta()<<" "<<digihitpos[ix].eta()<<endl;
    }
    Tout->Fill();
    cout<<"input3 "<<endl;
  } //end of file loop
  fileIn->cd();
  delete Tin;
  delete fileIn;

  fileOut->cd();
  fileOut->Write();

}
////////////////////////////////////////////////////////////////////////////////////
////
////                            Digitisation
////
////////////////////////////////////////////////////////////////////////////////////
void fill_digipos_array(int nhit, unsigned int* detid, float* simenr, float* simtime, vector<Hep3Vector>& hitpos) {
  //Decode detid to position and energy and efficiency to select hitpoint for fit 
  Hep3Vector tmp3vect;
  //  cout<<"nhit "<<nhit<<endl;
  for (int ij = 0;  ij<nhit; ij++) {
    if (simenr[ij]<=0) continue;
    if (gRandom3->Uniform()>0.95) continue;
    double rho = sirad[((detid[ij]>>28)&0xF)]; //Decode layer number, then position in metre

    int InThe = ((detid[ij]>>15)&0x1FFF); //Decode the digitised value for z
    double zz = (0.2*(InThe+0.5) - 750.0); // Z-position in mm
#ifdef SMEARING
    zz +=gRandom3->Gaus(0.0, 0.2); //Gaussian smareing of position 200 micron
#endif
    zz = zz/1000.0; //position in m (which is used in track fit)
    
    int InPhi = (detid[ij]&0x7FFF); //Decode the digitised value value of phi;
    double phi = ((InPhi+0.5)/20000.0 - pibyfour); // convert it to rad

#ifdef SMEARING
    //    phi +=gRandom3->Gaus(0.0, 0.05); //Gaussian smareing of position 0.02mrad
    //position resolution 100micron, 50, 100, 150, 200, 300, 500, 700, 1000
    double xx = gRandom3->Gaus(0.0, 1.e-4);
    phi +=xx/rho;

#endif

    tmp3vect.setRhoPhiZ(rho, phi, zz);
    //    cout<<"ijj "<< simenr[ij]<<" "<<rho<<" "<<phi<<" "<<zz<<endl;
    hitpos.push_back(tmp3vect); //Adding three vector in the list
  }
  
  //Randomly generated hit position and fill array
  // In real case, it should be indigitised form and also depend on detector condition
  // For simplicity (with endcoding and decoding of the/phi/z) directly generate the/phi/z
  for (int ij=0; ij<nNoiseHit; ij++) {
    double phi = (int((pibytwo*gRandom3->Uniform() - pibyfour)*20000+0.5))/20000.;
    double the = acos(sqrt(2)*(gRandom3->Uniform()-0.5));

    //    double xx = gRandom3->Uniform();
    //    while(xx<1./(nsilayer-1)) { // for f(r) = 1/r^2, 0 to infinty, r=r_{Minimum}/x
    //      xx = gRandom3->Uniform();
    //    }
    //    double rad = sirad[int(1./xx)]; //in metre
    double rad = sirad[int(nsilayer*gRandom3->Uniform())]; //in metre

    tmp3vect.setRhoPhiTheta(rad, phi, the);
    hitpos.push_back(tmp3vect);
  }
}