#include <string.h>
#include <iostream>
#include <vector>
#include "TFile.h"
#include "TLorentzVector.h"
#include "TH1.h"
#include "TH2.h"
#include "TF1.h"
#include "TRandom3.h"
#include "TVector3.h"                                                                           
#include <fstream>


using namespace std;

struct myGen_t {                                                     
   int runNumber;
   int eventNumber;
   double weight;
   vector<int> pidVecGeant;
   vector<int> pidVecPDG;
   vector<int> chargeVec;
   vector<TLorentzVector> mo4Vec;
   vector<TVector3> vertex3Vec;
};


//FUNCTION PROTOTYPES
void printUsage(); // print usage
void writeEventLund(myGen_t myGen,std::ofstream *lundFile);                          
void writeEventGenr8(myGen_t myGen,std::ofstream *genr8File);
void OneBodyDecay(TRandom3 *gRandom, double m_mom, double m_d1, double m_d2, TLorentzVector *D1, TLorentzVector *D2);



void tchannelCM(TRandom3 *gRandom,double tSlope,TLorentzVector p1CM,TLorentzVector p2CM,
		double m_3,double m_4,TLorentzVector *p3CM,TLorentzVector *p4CM, double *tmpVal);

//MAIN
int main(int argc, char **argv){
  
  char  inFileName[120];
  char  outFileName[120];
  char  outFileNameLund[120];                                                                   
  char  outFileNameGenr8[120];
  char *argptr;
  int nToRun = 100000;
  int rSeed = 0;
  
  //double pi = 3.14159;
  double mHyperon      = 2.153;//2.96;
  double mHyperonCenter= 2.153;//2.96;
  double gHyperon      = 0.107;//0.8458;
  double mKplus        = 0.4937; 
  double mXiStarCenter = 1.535;
  double mXiStar       = mXiStarCenter;
  double gXiStar       = 9.9/1000;
  double mpi0          = 0.1349768;
  double mXiGS         = 1.3217; 
  double mPro          = .938;
  double mpim          = 0.1395701;
  double mLambda       = 1.11568;
  

  TLorentzVector target(0,0,0,mPro);
  
  //Set the default output file
  sprintf(outFileName,"./myOutFile.root");
  sprintf(outFileNameLund,"./myOutFile.lund");                                                  
  sprintf(outFileNameGenr8,"./myOutFile.genr8");
  
  int outType  = 1;                                                                            
  int runNumber = 99999;
  
  for (int i=1; i<argc; i++) {
    argptr = argv[i];
    if (*argptr == '-') {
      argptr++;
      switch (*argptr) {
      case 'h':
	printUsage();
	break;
      case 'n':
	nToRun = atoi(++argptr);
	break;
      case 's':
	rSeed = atoi(++argptr);
	break;
      case 'o':
	strcpy(outFileName,++argptr);
	strcpy(outFileNameLund,++argptr);
	strcpy(outFileNameGenr8,++argptr);                                          
	break;
      case 't':                                                                                 
	outType = atoi(++argptr);                                                              
	break;                                                                     
      case 'r':                                                                               
	runNumber = atoi(++argptr);                                                             
	break;     
      default:
	printUsage();
	break;
      }
    } else {
      strcpy(inFileName,argptr);
    }
  }
  
  //Create histograms here
  TH1D *hDummy1d     = new TH1D("hDummy1d","",100,0.0,1.0);
  TH2D *hDummy2d     = new TH2D("hDummy2d","",100,0.0,1.0,100,0.0,1.0);
  
  TH1D * hEGamma     = new TH1D("hEGamma","",1200,0.0,12);  
  TH1D * hMassHyperon     = new TH1D("hMassHyperon","",1500,1.0,4.0);
  TH1D * hTVal            = new TH1D("hTVal","",200,0.0,20);  
  TH1D * hYStar               = new TH1D("hYStar","",3000,1.5,4.5);  

  TH1D * htValTrue            = new TH1D("htValTrue","",2000,0.0,20.0);
  TH1D * htValLow              = new TH1D("htValLow","",2000,0.0,20.0);
  TH1D * htValHigh             = new TH1D("htValHigh","",2000,0.0,20.0);

  TH1D * hYStar1              = new TH1D("hYStar1","",3000,1.5,4.5);
  TH1D * hYStar2              = new TH1D("hYStar2","",3000,1.5,4.5);

  TH2D * hYStar12             = new TH2D("hYStar12","",500,1.5,6.5,500,1.5,6.5);

  TH1D * hYStarSlow            = new TH1D("hYStarSlow","",3000,1.5,4.5);
  TH1D * hYStarFast            = new TH1D("hYStarFast","",3000,1.5,4.5);

  TH1D * hYStarSlowT            = new TH1D("hYStarSlowT","",3000,1.5,4.5);
  TH1D * hYStarFastT            = new TH1D("hYStarFastT","",3000,1.5,4.5);

  TH1D * hYStarLow            = new TH1D("hYStarLow","",3000,1.5,4.5);
  TH1D * hYStarHigh           = new TH1D("hYStarHigh","",3000,1.5,4.5);

  TH1D * hYStarLowT            = new TH1D("hYStarLowT","",3000,1.5,4.5);
  TH1D * hYStarHighT           = new TH1D("hYStarHighT","",3000,1.5,4.5);

  TH1D * hDeltaPx                  = new TH1D("hDeltaPx","",101,-1.0,1.0);
  TH1D * hDeltaPy                  = new TH1D("hDeltaPy","",101,-1.0,1.0);
  TH1D * hDeltaPz                  = new TH1D("hDeltaPz","",101,-1.0,1.0);
  TH1D * hDeltaE                   = new TH1D("hDeltaE","",101,-1.0,1.0);  
  
  TH2D * hMLambdaVsP          = new TH2D("hLambdaVsP","",300,1.0,1.3,1000,0.0,10.0);
  TH2D * hProtonMvsP          = new TH2D("hProtonMvsP","",300,0.7,1.0,1000,0.0,10.0);
  TH2D * hPiMinus1MvsP        = new TH2D("hPiMinus1MvsP","",400,0.0,0.2,1000,0.0,10.0);
  TH2D * hPiMinus2MvsP        = new TH2D("hPiMinus2MvsP","",400,0.0,0.2,1000,0.0,10.0);


  //Setup the random generator
  TRandom3 *gRandom =  new TRandom3();
  gRandom->SetSeed(rSeed);
  
  
  std::ofstream lundFile;
  std::ofstream genr8File;
  if (outType == 2) lundFile.open(outFileNameLund);
  if (outType == 3) genr8File.open(outFileNameGenr8);
  
  
  for(Int_t i=0;i<nToRun;i++){
    if (i%100000 == 0 && i > 0) cout<<"Events processed = "<<i<<endl;
    bool keep = false;
    double testVal = 0;
    double eGammaLow  = 7.0;
    double eGammaHigh = 11.0;
    double eGamma = gRandom->Uniform(eGammaLow,eGammaHigh);//Grab a photon energy
    hEGamma->Fill(eGamma);    
    TLorentzVector beam(0,0,eGamma,eGamma);
    TLorentzVector pTot4Vec = beam + target;
    //
    //beam and target to cm frame
    // 
    TLorentzVector beamCM = beam;
    beamCM.Boost(-pTot4Vec.BoostVector());
    TLorentzVector targetCM = target;
    targetCM.Boost(-pTot4Vec.BoostVector());
    //mHyperon = gRandom->Uniform(2.03,2.27);  
    mHyperon = gRandom->Uniform(2.03,3.0);  
    hMassHyperon->Fill(mHyperon);
    TLorentzVector Kaon1CM4Vec(0,0,0,0);
    TLorentzVector Kaon2CM4Vec(0,0,0,0);
    TLorentzVector HyperonCM4Vec(0,0,0,0);

    double tSlope = -1.05;
    double tmpVal = 0;
    tchannelCM(gRandom,tSlope,beamCM,targetCM,
	       mKplus,mHyperon,&Kaon1CM4Vec,&HyperonCM4Vec,&tmpVal);
    TLorentzVector tVec = beamCM - Kaon1CM4Vec;
    hTVal->Fill(fabs(tVec.Mag2()));

    TLorentzVector Kaon1_4Vec = Kaon1CM4Vec;
    Kaon1_4Vec.Boost(pTot4Vec.BoostVector());

    TLorentzVector Hyperon4Vec = HyperonCM4Vec;
    Hyperon4Vec.Boost(pTot4Vec.BoostVector());
    hYStar->Fill(Hyperon4Vec.M());

    /////////////Hyperon Decay YRF////////////////
    TLorentzVector XiStarYRF4Vec;
    TLorentzVector Kaon2YRF4Vec;

    OneBodyDecay(gRandom,mHyperon,mXiStar,mKplus,&XiStarYRF4Vec,&Kaon2YRF4Vec);

    TLorentzVector XiStar4Vec = XiStarYRF4Vec;
    XiStar4Vec.Boost(Hyperon4Vec.BoostVector());

    TLorentzVector Kaon2_4Vec = Kaon2YRF4Vec;
    Kaon2_4Vec.Boost(Hyperon4Vec.BoostVector());

    /////////////XiStar Decay XiStarRF/////////////////
    TLorentzVector XiMinus4Vec_XiStarRF;
    TLorentzVector Pi04Vec_XiStarRF;
    OneBodyDecay(gRandom,XiStar4Vec.M(),mXiGS,mpi0,&XiMinus4Vec_XiStarRF,&Pi04Vec_XiStarRF);

    TLorentzVector XiMinus4Vec = XiMinus4Vec_XiStarRF;
    XiMinus4Vec.Boost(XiStar4Vec.BoostVector());

    TLorentzVector Pi04Vec = Pi04Vec_XiStarRF;
    Pi04Vec.Boost(XiStar4Vec.BoostVector());
    
    //Decay the ground state cascade                                                                          
    
    TLorentzVector Lambda4Vec_XiMinusRF;
    TLorentzVector PiMinus4Vec_XiMinusRF;
    OneBodyDecay(gRandom,XiMinus4Vec.M(),mLambda,mpim,&Lambda4Vec_XiMinusRF,&PiMinus4Vec_XiMinusRF);

    TLorentzVector Lambda4Vec = Lambda4Vec_XiMinusRF;
    Lambda4Vec.Boost(XiMinus4Vec.BoostVector());

    TLorentzVector PiMinus4Vec = PiMinus4Vec_XiMinusRF;
    PiMinus4Vec.Boost(XiMinus4Vec.BoostVector());


    hMLambdaVsP->Fill(Lambda4Vec.M(),Lambda4Vec.P());
    hPiMinus1MvsP->Fill(PiMinus4Vec.M(),PiMinus4Vec.P());

    //Decay the Lambda

    TLorentzVector Proton4Vec_LambdaRF;
    TLorentzVector PiMinus_2_4Vec_LambdaRF;
    OneBodyDecay(gRandom,Lambda4Vec.M(),mPro,mpim,&Proton4Vec_LambdaRF,&PiMinus_2_4Vec_LambdaRF);
    TLorentzVector Proton4Vec = Proton4Vec_LambdaRF;
    Proton4Vec.Boost(Lambda4Vec.BoostVector());

    TLorentzVector PiMinus_2_4Vec = PiMinus_2_4Vec_LambdaRF;
    PiMinus_2_4Vec.Boost(Lambda4Vec.BoostVector());

    hProtonMvsP->Fill(Proton4Vec.M(),Proton4Vec.P());
    hPiMinus2MvsP->Fill(PiMinus_2_4Vec.M(),PiMinus_2_4Vec.P());
    


    /////////////

    TLorentzVector KLow;
    TLorentzVector KHigh;

    double theta_1 = Kaon1_4Vec.Theta()*180.0/3.14159;
    double theta_2 = Kaon2_4Vec.Theta()*180.0/3.14159;


    bool lowTrue = false;
    if(theta_1<theta_2)
      {
        KLow  = Kaon1_4Vec;
        KHigh = Kaon2_4Vec;
      }
    else
      {
        KLow  = Kaon2_4Vec;
        KHigh = Kaon1_4Vec;
      }
    TLorentzVector kFast;
    TLorentzVector kSlow;

    double v_k1 = Kaon1_4Vec.P()/Kaon1_4Vec.E();
    double v_k2 = Kaon2_4Vec.P()/Kaon2_4Vec.E();

    bool slowTrue = false;
    //NOTE Kaon2 is true                                                        
    if(v_k1 > v_k2)
      {
        kFast = Kaon1_4Vec;
        kSlow = Kaon2_4Vec;
	slowTrue = true;
      }
    else
      {
	kFast = Kaon2_4Vec;
        kSlow = Kaon1_4Vec;
      }

    TLorentzVector Hyperon1 = XiStar4Vec + Kaon1_4Vec;
    TLorentzVector Hyperon2 = XiStar4Vec + Kaon2_4Vec;

    TLorentzVector HyperonLow  = XiStar4Vec + KLow ;
    TLorentzVector HyperonHigh = XiStar4Vec + KHigh;
    TLorentzVector HyperonSlow  = XiStar4Vec + kSlow ;
    TLorentzVector HyperonFast = XiStar4Vec + kFast;

    TLorentzVector tValTrue = beam-Kaon1_4Vec;
    TLorentzVector tValLow = beam-KLow;
    TLorentzVector tValHigh = beam-KHigh;

    htValTrue->Fill(-tValTrue.Mag2());
    htValLow->Fill(-tValLow.Mag2());
    htValHigh->Fill(-tValHigh.Mag2());

    hYStarFast->Fill(HyperonFast.M());
    hYStarSlow->Fill(HyperonSlow.M());
 
    hYStarLow->Fill(HyperonLow.M());
    hYStarHigh->Fill(HyperonHigh.M());

    if (slowTrue == false) hYStarFastT->Fill(Hyperon2.M());
    if (slowTrue == true) hYStarSlowT->Fill(HyperonSlow.M());

    if (lowTrue == true) hYStarLowT->Fill(HyperonLow.M());
    if (lowTrue == false) hYStarHighT->Fill(HyperonHigh.M());

    hYStar1->Fill(Hyperon1.M());
    hYStar2->Fill(Hyperon2.M());
    hYStar12->Fill(Hyperon1.M(),Hyperon2.M());

    /////////////


    TVector3 vertVec(0,0,0);
    //Load the myGen structure with final-state particles                                                      
    myGen_t myGen;
    myGen.weight      = 1.0;
    myGen.runNumber   = runNumber;
    myGen.eventNumber = i+1;
    myGen.vertex3Vec.push_back(vertVec);
    //K+ 1                                                                                                     
    myGen.mo4Vec.push_back(Kaon1_4Vec);
    myGen.pidVecPDG.push_back(321);
    myGen.pidVecGeant.push_back(11);
    myGen.chargeVec.push_back(1);
    //K+ 2                                                                                                      
    myGen.mo4Vec.push_back(Kaon2_4Vec);
    myGen.pidVecPDG.push_back(321);
    myGen.pidVecGeant.push_back(11);
    myGen.chargeVec.push_back(1);
    //Pi0                                                                                                      
    myGen.mo4Vec.push_back(Pi04Vec);
    myGen.pidVecPDG.push_back(111);
    myGen.pidVecGeant.push_back(7);
    myGen.chargeVec.push_back(0);
    //Pi-Minus                                                                                                 
    myGen.mo4Vec.push_back(PiMinus4Vec);
    myGen.pidVecPDG.push_back(-211);
    myGen.pidVecGeant.push_back(9);
    myGen.chargeVec.push_back(-1);
    //Proton                                                                                                   
    myGen.mo4Vec.push_back(Proton4Vec);
    myGen.pidVecPDG.push_back(2212);
    myGen.pidVecGeant.push_back(14);
    myGen.chargeVec.push_back(1);
    //PiMinus2                                                                                                 
    myGen.mo4Vec.push_back(PiMinus_2_4Vec);
    myGen.pidVecPDG.push_back(-211);
    myGen.pidVecGeant.push_back(9);
    myGen.chargeVec.push_back(-1);

    if (outType == 2) writeEventLund(myGen, &lundFile);
    if (outType == 3) writeEventGenr8(myGen, &genr8File);

    //START TEST                                                                                               
    TLorentzVector x4Vec(0,0,0,0);
    x4Vec = beam + target;


    for (int i = 0; i<(int)myGen.pidVecGeant.size(); i++){
      x4Vec -= myGen.mo4Vec[i];
    }

    hDeltaPx->Fill(x4Vec.Px());
    hDeltaPy->Fill(x4Vec.Py());
    hDeltaPz->Fill(x4Vec.Pz());
    hDeltaE->Fill(x4Vec.E());
    //END TEST           
    
  }

  if (outType == 1){
    TFile *outFile = new TFile(outFileName,"RECREATE");          
    hMassHyperon->Write();
    hEGamma->Write();
    hTVal->Write();
    hYStar->Write();
    hYStar1->Write();
    hYStar2->Write();
    hYStar12->Write();

    hYStarLow->Write();
    hYStarHigh->Write();
    hYStarSlow->Write();
    hYStarFast->Write();

    hYStarSlowT->Write();
    hYStarFastT->Write();

    hYStarLowT->Write();
    hYStarHighT->Write();

    hYStarLow->Write();
    hYStarHigh->Write();

    htValTrue->Write();
    htValLow->Write();;
    htValHigh->Write();

    hDeltaPx->Write();
    hDeltaPy->Write();
    hDeltaPz->Write();
    hDeltaE->Write();


    hMLambdaVsP->Write();
    hProtonMvsP->Write();
    hPiMinus1MvsP->Write();
    hPiMinus2MvsP->Write();



    outFile->Write();
  }
  
  if (outType == 2) lundFile.close();                                               
  if (outType == 3) genr8File.close();
  
  return 0;
}

void printUsage(){
  fprintf(stderr,"\nUsage:");
  fprintf(stderr,"\nmyEventGen [-switches]\n");
  fprintf(stderr,"\nSWITCHES:\n");
  fprintf(stderr,"-h\tPrint this message\n");
  fprintf(stderr,"-s<arg>\tRandom seed. Default is 0\n");
  fprintf(stderr,"-o<arg>\tOutFileName. Default is myOutFile.txt\n\n");
  fprintf(stderr,"-r<arg>\tRun number. Default is 99999\n\n");
  fprintf(stderr,"-t<arg>\tOutFile type: 1->Root, 2->Lund, 3->Genr8\n\n");

  cout<<"The current default operation is equivalent to the command:"<<endl;
  //  cout<<"myEventGen -s0 -n100000 -omyOutFile.root\n"<<endl;
  cout<<"myEventGen -s0 -t1 -r99999 -n100000 -omyOutFile.root\n"<<endl;
  exit(0);
}

void writeEventLund(myGen_t myGen, std::ofstream *lundFile){
 //See https://gemc.jlab.org/gemc/html/documentation/generator/lund.html                 
 //LUND HEADER                                                                           
 *lundFile<<myGen.pidVecPDG.size();//  1) Number of particles                            
 *lundFile<<"\t0.938";             //  2) Mass of target:          NOT REQUIRED          
 *lundFile<<"\t1";                 //  3) Atomic number of target: NOT REQUIRED          
 *lundFile<<"\t0";                 //  4) Target polarization:     NOT REQUIRED          
 *lundFile<<"\t0";                 //  5) Beam polarization                              
 *lundFile<<"\t11";                //  6) Beam type:               NOT REQUIRED          
 *lundFile<<"\t10";                //  7) Beam energy:             NOT REQUIRED          
 *lundFile<<"\t2212";              //  8) Interacted nucleon ID:   NOT REQUIRED          
 *lundFile<<"\t0";                 //  9) Process ID:              NOT REQUIRED          
 *lundFile<<"\t"<<myGen.weight;    // 10) Event weight:            NOT REQUIRED          
 endl(*lundFile); //Write the header            
 //LUND particle                                                                         
 for (int i = 0; i<(int)myGen.pidVecPDG.size(); i++){
   *lundFile<<i+1;                          //  1) Particle index                        
   *lundFile<<"\t0";                        //  2) Lifetime:            NOT REQUIRED     
   *lundFile<<"\t1";                        //  3) Type (1 is active)                    
   *lundFile<<"\t"<<myGen.pidVecPDG[i];     //  4) Particle ID                           
   *lundFile<<"\t0";                        //  5) Index of parent:     NOT REQUIRED     
   *lundFile<<"\t-1";                       //  6) Index of daughter:   NOT REQUIRED     
   *lundFile<<"\t"<<myGen.mo4Vec[i].Px();   //  7) Momentum in x (GeV)                   
   *lundFile<<"\t"<<myGen.mo4Vec[i].Py();   //  8) Momentum in y (GeV)                   
   *lundFile<<"\t"<<myGen.mo4Vec[i].Pz();   //  9) Momentum in z (GeV)                   
   *lundFile<<"\t"<<myGen.mo4Vec[i].E();    // 10) Energy (GeV):        NOT REQUIRED     
   *lundFile<<"\t"<<myGen.mo4Vec[i].Mag();  // 11) Mass (GeV):          NOT REQUIRED     
   *lundFile<<"\t"<<myGen.vertex3Vec[i].X();// 12) Vertex x (cm)                         
   *lundFile<<"\t"<<myGen.vertex3Vec[i].Y();// 12) Vertex x (cm)                         
   *lundFile<<"\t"<<myGen.vertex3Vec[i].Z();// 12) Vertex x (cm)                         
   endl(*lundFile); //Write the particle                                                 
 }
}

void writeEventGenr8(myGen_t myGen, std::ofstream *genr8File){
 //See page 5 of https://halldweb.jlab.org/DocDB/0000/000011/001/genr8_v_1_0.pdf         
 //Genr8 HEADER                                                                          
 *genr8File<<myGen.runNumber;                // 1) Run number                            
 *genr8File<<"\t"<<myGen.eventNumber;        // 2) Event number                          
 *genr8File<<"\t"<<myGen.pidVecGeant.size(); // 3) Number of particles                   
 endl(*genr8File); //Write the header                                                    
 //Genr8 particle                                                                        
 for (int i = 0; i<(int)myGen.pidVecGeant.size(); i++){
   *genr8File<<i+1;                          // 1) Particle index                        
   *genr8File<<"\t"<<myGen.pidVecGeant[i];   // 2) Particle ID                           
   *genr8File<<"\t"<<myGen.mo4Vec[i].Mag();  // 3) Mass                                  
   endl(*genr8File);                         //Write line                                
   *genr8File<<"\t"<<myGen.chargeVec[i];     // 1) Charge                                
   *genr8File<<"\t"<<myGen.mo4Vec[i].Px();   // 2) Momentum in x                         
   *genr8File<<"\t"<<myGen.mo4Vec[i].Py();   // 3) Momentum in y                         
   *genr8File<<"\t"<<myGen.mo4Vec[i].Pz();   // 4) Momentum in z                         
   *genr8File<<"\t"<<myGen.mo4Vec[i].E();    // 5) Energy                                
   endl(*genr8File); //Write the particle                                                
 }
}


void OneBodyDecay(TRandom3 *gRandom,double m_mom, double m_d1, double m_d2, TLorentzVector *D1, TLorentzVector *D2)
{
  double pi = 3.14159;
  double energy_D1 = (pow(m_mom,2)+pow(m_d1,2)-pow(m_d2,2))/(2*m_mom);
  double p_D1      = sqrt(pow(energy_D1,2)-pow(m_d1,2));
  
  double energy_D2 = m_mom - energy_D1; 

  double phi      = gRandom->Uniform(-pi,pi);    
  double cosTheta = gRandom->Uniform(-1.0,1.0);
  
  double p_D1_ZCM           = p_D1*cosTheta;
  double p_D1_Transverse    = sqrt(pow(p_D1,2) - pow(p_D1_ZCM,2));
  double p_D1_XCM           = p_D1_Transverse*cos(phi);
  double p_D1_YCM           = p_D1_Transverse*sin(phi);

  double p_D2_ZCM           = -p_D1_ZCM;
  double p_D2_XCM           = -p_D1_XCM;
  double p_D2_YCM           = -p_D1_YCM;           

  *D1 =  TLorentzVector(p_D1_XCM,p_D1_YCM,p_D1_ZCM,energy_D1);
  *D2 =  TLorentzVector(p_D2_XCM,p_D2_YCM,p_D2_ZCM,energy_D2);

}

void tchannelCM(TRandom3 *gRandom,double tSlope,TLorentzVector p1CM,TLorentzVector p2CM,
		double m_3,double m_4,TLorentzVector *p3CM,TLorentzVector *p4CM,double *tmpVal){
  double pi = 3.14159;
  
  //Find t0, t1
  double m_1 = p1CM.Mag();
  double m_2 = p2CM.Mag();

  TLorentzVector wVec = p1CM + p2CM;
  double wVal = wVec.Mag(); 
  double sVal = wVec.Mag2(); 

  double p1_p = p1CM.P();

  double p3_e = (sVal + pow(m_3,2) - pow(m_4,2))/(2*wVal);
  double p3_p = sqrt(pow(p3_e,2) - pow(m_3,2));

  double p4_p = p3_p;
  double p4_e = sqrt(pow(p4_p,2)+pow(m_4,2));

  //Start of finding t0, t1
  double t0 = pow(pow(m_1,2) - pow(m_3,2) - pow(m_2,2) + pow(m_4,2),2)/(4*sVal) 
    - pow(p1_p - p3_p,2);

  double t1 = pow(pow(m_1,2) - pow(m_3,2) - pow(m_2,2) + pow(m_4,2),2)/(4*sVal) 
    - pow(p1_p + p3_p,2);
  //End of finding t0, t1


  double tVal=0;
  bool keep = false;
  double expMax = exp(tSlope*fabs(t1));
  double expMin = exp(tSlope*fabs(t0));
  while (keep == false){      
    tVal = gRandom->Uniform(t1,t0);     
    double expTest    = exp(tSlope*fabs(tVal));
    double yVal = gRandom->Uniform(0,1);
    if (yVal < expTest) keep = true;
  }
  *tmpVal = tVal;

  double zVal = (t0-tVal)/(4*p1_p*p3_p);
  double theta= 2*asin(sqrt(zVal));

  double phi      = gRandom->Uniform(-pi,pi);    
  double cosTheta = cos(theta);
  
  double p3_z          = p3_p*cosTheta;
  double p3_Transverse = sqrt(pow(p3_p,2) - pow(p3_z,2));
  double p3_x          = p3_Transverse*cos(phi);
  double p3_y          = p3_Transverse*sin(phi);

  double p4_z          = -p3_z;
  double p4_x          = -p3_x;
  double p4_y          = -p3_y;

  p3CM->SetXYZT(p3_x,p3_y,p3_z,p3_e);
  p4CM->SetXYZT(p4_x,p4_y,p4_z,p4_e);

}