//_HADES_CLASS_DESCRIPTION 
////////////////////////////////////////////////////////////////////////////
//*-- AUTHOR : G. Kornakov
//
// HParticleT0Reco
//
// Reconstructor to recalulate the T0 form the reconstructed particle
// to increase the time resolution. By default T0 is calulated form all particles
// flagged as kIsUsed. The user has to take care that enough used particles
// are avalilable. As standard hadrons and leptons are flagged. As alternative
// all candidates with fitted inner/outer segment,RKchi2<1000,MetaHit and beta!=-1
// can be used when setUseFlaggedCandidates(kFALSE) is used.
//
//  setDoPathLengthCorr(kTRUE) (default) performs the path length correction
//  for the vertex position before running T0 reco.
//
//  setBeamTime() has to be set for the correct data set (default "apr12")
//
//--------------------------------------------------------------------------
// USAGE:
//
//  HParticleT0Reco t0Reco;
//  t0Reco.setDoPathLengthCorr(kTRUE);
//  t0Reco.setUseFlaggedCandidates(kTRUE);
//  t0Reco.setBeamTime("apr12");
//
//  // 2 possible ways :
//  a. As Reconstructor add it to standard task list
//  b. With HLoop :
//
//     before eventLoop :  t0Reco.init();
//     inside eventloop :  t0Reco.execute();
//
//
////////////////////////////////////////////////////////////////////////////



#include "hparticlet0reco.h"

#include "hades.h"
#include "hevent.h"
#include "hphysicsconstants.h"
#include "hcategory.h"
#include "hparticlecand.h"
#include "hstart2hit.h"
#include "hstartdef.h"


ClassImp(HParticleT0Reco)


HParticleT0Reco::HParticleT0Reco(const Text_t* name,const Text_t* title)
    : HReconstructor(name,title) {
	run = "apr12";
	setDoPathLengthCorr(kTRUE);
	c = 299.792458;
	t0rpc = 0.;
	t0tof = 0.;
	fUseFlagged =kTRUE;
}

HParticleT0Reco::~HParticleT0Reco() {
}

Int_t HParticleT0Reco::execute(){
    fill();
    return 0;
}

Bool_t HParticleT0Reco::init()
{
    candCat  = gHades->getCurrentEvent()->getCategory(catParticleCand);
    catStart = gHades->getCurrentEvent()->getCategory(catStart2Hit);
    if (!candCat) {
	Error("init","No ParticleCand Category");
    }

    fpathCorr.setBeamTime(run);
    fpathCorr.init();

    return kTRUE;
}

void HParticleT0Reco::setBeamTime(TString beamTime)
{
    beamTime.ToLower();
    if(beamTime == "apr12") {
	run = beamTime;
    } else {
	run = "";
	Error("setBeam()","unknown beam time %s!",beamTime.Data());
    }
}

Double_t HParticleT0Reco::masspp(Double_t x)
{
    if(x<267.341) {
	return 120.831 + x*0.346257 - x*x*0.00258412 + x*x*x*8.31672e-06 - x*x*x*x*3.16719e-9 - x*x*x*x*x*5.01785e-11 + x*x*x*x*x*x*1.26139e-13 - x*x*x*x*x*x*x*9.62809e-17;
    } else {
	return 139.57;
    }
    return 0.;
}

Double_t HParticleT0Reco::masspm(Double_t x)
{
    if(x<316.602) {
	return 131.378 + x*0.0830105 + x*x*-0.000289921 + x*x*x*-6.09662e-08 + x*x*x*x*1.68545e-09 + x*x*x*x*x*1.02188e-12 + x*x*x*x*x*x*-1.02844e-14 + x*x*x*x*x*x*x*9.65495e-18;
    } else {
	return 139.57;
    }
    return 0.;
}

Double_t HParticleT0Reco::masspr(Double_t x)
{
    if(x<350.0) {
	return masspr(350.0);
    } else if (x<1329.67) {
	return 883.035 + x*0.146625  -1.75153e+06 * exp(-0.0342716*x) - x*x*0.000143704 + x*x*x*4.86392e-8;
    } else {
	return 938.272;
    }
    return 0.0; // should not happen
}

void HParticleT0Reco::fillTof(Float_t t0, Float_t err)
{
    t0fortof.push_back(t0);
    t0errfortof.push_back(err);
}

void HParticleT0Reco::fillRpc(Float_t t0, Float_t err)
{
    t0forrpc.push_back(t0);
    t0errforrpc.push_back(err);
}

void HParticleT0Reco::fillBuffer() {
    fillRpc(getT0rpc(),0.);
    fillTof(getT0tof(),0.);
    // If the buffer is already larger than 1000, empty the first!
    if(t0fortof.size()>1000)t0fortof.pop_front();
    if(t0forrpc.size()>1000)t0forrpc.pop_front();
}

void HParticleT0Reco::fillCorrections()
{
    t0tof = 0.;
    t0rpc = 0.;
    if(t0fortof.size()>2) {
	for(list<Float_t>::iterator it = t0fortof.begin();it!=t0fortof.end();it++) {
            t0tof+=*it;
	}
	t0tof/=t0fortof.size();
    } else  t0tof=0.;
    if(t0forrpc.size()>2) {
	for(list<Float_t>::iterator it = t0forrpc.begin();it!=t0forrpc.end();it++) {
            t0rpc+=*it;
	}
        t0rpc/=t0forrpc.size();
    } else  t0rpc=0.;
}



void HParticleT0Reco::fill()
{

    if(fdoPathLengthCorr) fpathCorr.execute();

    times.clear();
    index.clear();
    error.clear();
    timesTof.clear();
    indexTof.clear();
    errorTof.clear();
    HParticleCand* pCand=0;
    if(candCat)
    {
	for(Int_t j=0;j<candCat->getEntries();j++)
	{
	    pCand = (HParticleCand*)candCat->getObject(j);
	    if(!pCand) continue;

	    if(fUseFlagged) {
		if (!pCand->isFlagBit(Particle::kIsUsed)) continue;
	    }
	    else {

		if(pCand->getBeta()==-1 || pCand->getChi2() > 1000 || !pCand->isFlagAND(4,
											Particle::kIsAcceptedHitInnerMDC,
											Particle::kIsAcceptedHitOuterMDC,
											Particle::kIsAcceptedHitMETA,
											Particle::kIsAcceptedRK) ) continue;
	    }
	    if(pCand->isFakeRejected()) continue;
	    if(pCand->getChi2()>500.) continue;
	    Float_t mom         = pCand->getMomentum();
	    Float_t beta        = pCand->getBeta();
	    Float_t distMeta    = pCand->getDistanceToMetaHit();

	    Bool_t kIsPiP = isParticle(8,pCand,4);
	    Bool_t kIsPiM = isParticle(9,pCand,4);
	    Bool_t kIsPr  = isParticle(14,pCand,4);

	    Bool_t kExtra = 0;
	    if(pCand->getMetaCell(1)>=0 && pCand->getMetaModule(1)>=0) kExtra = 1;

	    Float_t tofmom  = -100000.;
	    Float_t betamom  = -100000.;
	    Float_t sigma    = 100.;

	    if(run == "apr12") {
		// Get RPC dt value
		if(pCand->getSelectedMeta()==3) {
		    if(kIsPr) {
			betamom = mom/sqrt(mom*mom+HPhysicsConstants::mass(14)*HPhysicsConstants::mass(14));
			if(mom<1320.)
			    tofmom = distMeta/betamom/c + (-1.45473e+11 * pow(mom, -4.57097) +-0.0103141);
			else
			    tofmom = distMeta/betamom/c + (-0.029812 + -2.03034e-05 * mom +3.26173e-8 * mom*mom +-4.72905e-12 * mom*mom*mom);
			sigma  = 3.57085 * exp( mom * -0.00550985) +0.171354;
		    } else if(kIsPiP) {
			betamom = mom/sqrt(mom*mom+HPhysicsConstants::mass(8)*HPhysicsConstants::mass(8));
			tofmom = distMeta/betamom/c + (-21.1115 * pow(mom, -1.33674));
			sigma  = 0.658892 * exp( mom * -0.0193493) +0.112036;
		    }
		    if(kIsPiM) {
			betamom = mom/sqrt(mom*mom+HPhysicsConstants::mass(9)*HPhysicsConstants::mass(9));
			tofmom = distMeta/betamom/c + (-1.88086 * pow(mom, -0.94041) );
			sigma  = 0.197659 * exp( mom * -0.0123527) +0.109623;
		    }
		}
		// Get TOF dt value
		if(pCand->getSelectedMeta()!=3) {
		    if(kIsPr) {
			betamom = mom/sqrt(mom*mom+HPhysicsConstants::mass(14)*HPhysicsConstants::mass(14));
			if(mom<1000) tofmom = distMeta/betamom/c + (-1.45519e+11 * pow(mom, -4.56364) +0.00338516 );
			else tofmom = distMeta/betamom/c + (0.671587 + -0.00158045 * mom + 1.16691e-06 * mom*mom  -2.53177e-10 * mom*mom*mom);
			sigma  = 0.143668 * exp( mom * -0.00435065) + 0.184781;
		    } else if(kIsPiP) {
			betamom = mom/sqrt(mom*mom+HPhysicsConstants::mass(8)*HPhysicsConstants::mass(8));
			tofmom = distMeta/betamom/c + (-3.28666e+8 * pow(mom, -4.69207) );
			sigma  = 0.143668 * exp( mom * -0.00435065) + 0.184781;
		    }
		    if(kIsPiM) {
			betamom = mom/sqrt(mom*mom+HPhysicsConstants::mass(9)*HPhysicsConstants::mass(9));
			tofmom = distMeta/betamom/c + (-2710.38 * pow(mom, -2.21636));
			sigma  = 0.135674 * exp( mom * -0.00911526) + 0.21758;
		    }
		}
	    }

	    if(tofmom<6. || tofmom>35.) continue;
	    if(sigma==0) {
		sigma = 10.;
	    }
	    Float_t dtime = distMeta/beta/c - tofmom;
	    if(dtime!=dtime) Error("fill()","T0Reco dtime corrupted! NaN!! ");

	    // Push back values!
	    if(pCand->getSelectedMeta()==3) {
		if(kIsPr) {
		    times.push_back(dtime);
		    index.push_back(pCand->getIndex());
		    error.push_back(1./sigma/sigma);
		}
		else if(kIsPiP) {
		    times.push_back(dtime);
		    index.push_back(pCand->getIndex());
		    error.push_back(1./sigma/sigma);
		}
		if(kIsPiM) {
		    times.push_back(dtime);
		    index.push_back(pCand->getIndex());
		    error.push_back(1./sigma/sigma);
		}
	    }
	    if(pCand->getSelectedMeta()<3) {
		if(kIsPr) {
		    timesTof.push_back(dtime);
		    indexTof.push_back(pCand->getIndex());
		    errorTof.push_back(1./sigma/sigma);
		}
		else if(kIsPiP) {
		    timesTof.push_back(dtime);
		    indexTof.push_back(pCand->getIndex());
		    errorTof.push_back(1./sigma/sigma);
		}
		if(kIsPiM) {
		    timesTof.push_back(dtime);
		    indexTof.push_back(pCand->getIndex());
		    errorTof.push_back(1./sigma/sigma);
		}
	    }
	}
    }
    fillBuffer();
    fillCorrections();
    correctBeta(); // Just fix beta in particleCand, measured time is not going to be modified, neither START.

}

void HParticleT0Reco::correctBeta()
{

    // is a two step procedure; first recalculate all betas, then use them to modify the category values.
    // Othwerwise, the modified beta value would modify the output of the loop if done in just one single step!

    vector <Float_t> betas;
    HParticleCand* pCand=0;
    Int_t i = 0;
    if(candCat) {
	for(Int_t j=0;j<candCat->getEntries();j++){
	    pCand = (HParticleCand*)candCat->getObject(j);
	    if(!pCand) continue;
	    betas.push_back(getBeta(pCand));
	}
    }
    if(candCat) {
	for(Int_t j=0;j<candCat->getEntries();j++){
	    pCand = (HParticleCand*)candCat->getObject(j);
	    if(!pCand) continue;
	    if(pCand->getBeta()!=-1 && pCand->isFlagAND(4,
							Particle::kIsAcceptedHitInnerMDC,
							Particle::kIsAcceptedHitOuterMDC,
							Particle::kIsAcceptedHitMETA,
							Particle::kIsAcceptedRK) ) {
		pCand->setBeta(betas[i]);
		Float_t mom = pCand->getMomentum();
		Float_t mass2 = mom*mom/betas[i]/betas[i]*(1.-betas[i]*betas[i]);
		pCand->setMass2(mass2);
	    }
	    i++;
	}
    }
}


Float_t HParticleT0Reco::getBeta(HParticleCand* pCand)
{
    Float_t t0 = 0;
    Float_t err = 0;
    Int_t size = times.size();
    if(times.size()<1 || pCand->getBeta() == -1) return pCand->getBeta();
    if(catStart)
    {
	HStart2Hit* sH=0;
	if((sH = (HStart2Hit *) catStart->getObject(0)) != NULL){
	    Int_t flag = sH->getCorrFlag();
	    if(pCand->getSelectedMeta()<3) t0 += -t0tof;
	    if(pCand->getSelectedMeta()==3) t0 += -t0rpc;
	    if(flag==2) {
		err+=1/0.12/0.12/2.;
	    }
	    if(flag<2) {
		err+=1./0.12/0.12/2.;
	    }
	}
    }
    for(UInt_t i=0;i<times.size();i++) {
	if(pCand->getIndex()!=index[i]) {
	    //Skip the particle for the t0 calculation!
	    t0  += times[i]*error[i];
	    err += error[i];
	}
	if(pCand->getIndex()==index[i]) {
	    size -= 1;
	}

    }
    if(err!=0)
	t0 = t0/err;
    else t0 = 0.;
    if(pCand->getSelectedMeta()==3) {
	//t0 -= t0rpc;
    }

    if (pCand->getSelectedMeta()<3) {
	t0 += (t0tof-t0rpc);
	//t0 = getT0tof();
    }
    Float_t oldtof  = pCand->getDistanceToMetaHit()/pCand->getBeta()/c;
    Float_t newBeta = pCand->getDistanceToMetaHit()/(oldtof - t0)/c;
    return newBeta;
}

Float_t HParticleT0Reco::getT0rpc()
{
    Float_t t0 = 0;
    Float_t err = 0;
    if(times.size()<1) return t0;

    for(UInt_t i=0;i<times.size();i++) {
	t0  += times[i]*error[i];
	err += error[i];
    }
    t0 = t0/err;
    return t0;
}

Float_t HParticleT0Reco::getT0tof()
{
    Float_t t0 = 0;
    Float_t err = 0;
    if(timesTof.size()<1) return t0;

    for(UInt_t i=0;i<timesTof.size();i++) {
	t0  += timesTof[i]*errorTof[i];
	err += errorTof[i];
    }
    t0 = t0/err;
    return t0;
}

Bool_t HParticleT0Reco::isParticle(Int_t PID, HParticleCand* pCand, Float_t sigma)
{

    if(!pCand) return kFALSE;
    if(pCand->getCharge()== 0) return kFALSE;
    if(pCand->getBeta()  ==-1) return kFALSE;
    if(pCand->getSystem()== 0 && pCand->getRpcInd()<0 ) return kFALSE;
    if(pCand->getSystem()== 1 && pCand->getTofHitInd()<0 && pCand->getTofClstInd()<0 ) return kFALSE;

    if(pCand->getCharge()>0 && (PID==9)   ) {
	return kFALSE;
    }
    if(pCand->getCharge()<0 && (PID==8 || PID==14)   ) {
	return kFALSE;
    }

    Float_t mom = pCand->getMomentum();
    Int_t   q   = pCand->getCharge();
    Int_t   sys = -1;
    if(pCand->getSelectedMeta()==3)
	sys = 0;
    if(pCand->getSelectedMeta()<3&&pCand->getSelectedMeta()>=0)
	sys = 1;
    if(sys<0)return kFALSE;

    Float_t   b   = pCand->getBeta();
    Float_t maxbeta=-1.0;
    Float_t minbeta=-1.0;

    if(q<0 && mom<85 && sys==0) return kFALSE;
    if(q<0 && mom<85 && sys==1) return kFALSE;
    if(q>0 && mom<60 && sys==0) return kFALSE;
    if(q>0 && mom<60 && sys==1) return kFALSE;

    // hardcoded params for apr12!!!!!!!!!!!!!!!!!!!!!!!!
    if(sys == 0 && PID == 8   && mom < 75.) return kFALSE;
    if(sys == 0 && PID == 9   && mom < 84.) return kFALSE;
    if(sys == 0 && PID == 14  && mom < 291.) return kFALSE;

    if(sys == 1 && PID == 8   && mom < 48.) return kFALSE;
    if(sys == 1 && PID == 9   && mom < 70.) return kFALSE;
    if(sys == 1 && PID == 14  && mom < 157.) return kFALSE;

    Float_t                    m = 0;
    if(sys == 0 && PID == 8 )  m = masspp(mom);      // Mass rpc
    if(sys == 0 && PID == 9 )  m = masspm(mom);      // Mass rpc
    if(sys == 0 && PID == 14 ) m = masspr(mom);      // Mass rpc

    if(sys == 1 && PID == 8 )  m = 139.57;           // Mass tof
    if(sys == 1 && PID == 9 )  m = 139.57;           // Mass tof
    if(sys == 1 && PID == 14 ) m = 938.272;          // Mass tof

    Float_t sT    = 0.12;      // sigma T rpc before any correction!
    if(sys==1) sT = 0.24;      // sigma T tof before any correction!
    Float_t sMom  = 0.01;      // sigma Mom 1%
    Float_t mT    = pCand->getDistanceToMetaHit()/b/299.792458; // time of flight used for beta uncertanty calculation

    Float_t nS    = 5.;        // N sigmas for mass lines! //Is tuned for event t0
    nS = sigma;

    if(PID!=2 && PID!=3) {
	maxbeta = mom/sqrt(mom*mom+m*m) + nS*sqrt( pow((sT/((mT/mom*sqrt(mom*mom+m*m) )-sT)),2)
						  + pow(( (mom*(1.+sMom))/sqrt((mom*(1.+sMom))*(mom*(1.+sMom))+m*m)
							 -mom/sqrt(mom*mom+m*m) ),2));
	minbeta = mom/sqrt(mom*mom+m*m) - nS*sqrt( pow((sT/((mT/mom*sqrt(mom*mom+m*m) )+sT)),2)
						  + pow(( (mom*(1.-sMom))/sqrt((mom*(1.-sMom))*(mom*(1.-sMom))+m*m)
							 -mom/sqrt(mom*mom+m*m) ),2));

	// [0] nsigmas
	// [1] sigmatof
	// [2] tof norm 7.0 tof 7.666 rpc
	// [3] mass -> m0 or from mass fit!
	// [4] momentum uncertainty 1.01 = +1% 0.99 = -1%

    }
    if(b>minbeta && b< maxbeta) return kTRUE;
    else return kFALSE;
}