void Boost_To_Stop_Rest_Frame(TLorentzVector& stop4, TLorentzVector& chargino4, TLorentzVector& b4, TLorentzVector& neutralino4, TLorentzVector& W4, TLorentzVector& up4, TLorentzVector& down4, TLorentzVector& s4)
{
    TVector3 betaV(-stop4.Px()/stop4.Energy(),-stop4.Py()/stop4.Energy(),-stop4.Pz()/stop4.Energy());
    stop4.Boost(betaV);
    chargino4.Boost(betaV);
    b4.Boost(betaV);
    neutralino4.Boost(betaV);
    W4.Boost(betaV);
    up4.Boost(betaV);
    down4.Boost(betaV);
    s4.SetE(chargino4.P()/chargino4.M());
    s4.SetVect(chargino4.Vect().Unit()*chargino4.Gamma());
}
	Disposable<Array> CmsCostFunction::values(const QuantLib::Array& x) const {

		Array result(m_+1);

		double beta1=0.0,beta2=0.0,nu1=0.0,nu2=0.0,lambda=0.0;
		bool sabrRecal;
		double t1=optPillarTimes_[skipOptPillars_];
		double t2=optPillarTimes_[noOptPillars_-1];
		if(mode_==0 || mode_==1) {
			beta1=(atan(x[0])+M_PI/2.0) / M_PI;
			beta2=(atan(x[1])+M_PI/2.0) / M_PI;
		}
		if(mode_==4 || mode_==5) {
			nu1=x[0]*x[0];
			nu2=x[1]*x[1];
		}
		if(mode_==1 || mode_==5) {
			lambda=x[2];
		}
		if(mode_==0 || mode_==1 || mode_==2 || mode_==3) {
			sabrRecal=true;
		}

#ifdef CMSHLOGGING
		FILE *out=fopen("costCms.log","a");
		fprintf(out,"UndPillar=%d nu=[%f,%f] beta=[%f,%f] lambda=%f\n", undPillarIndex_,nu1,nu2,beta1,beta2,lambda);
#endif

		double pen=0.0;
		std::vector<double> betaV(skipOptPillars_), nuV(skipOptPillars_);
		for(int i=skipOptPillars_;i<noOptPillars_;i++) {
			double nu=0.0,beta=0.0;
			if(mode_==0) {
				beta = beta1+(beta2-beta1)/(t2-t1)*(optPillarTimes_[i]-t1);
			}
			if(mode_==1) {
				beta = beta2+(beta1-beta2)*exp(-lambda*(optPillarTimes_[i]-t1));
			}
			if(mode_==2) {
				beta = (atan(x[i])+M_PI/2.0) / M_PI;
			}
			if(mode_==3) {
				beta = (x[0]+x[1]*(optPillarTimes_[i]-t1))*exp(-x[2]*(optPillarTimes_[i]-t1))+x[3];
				if(beta<0.01) beta=0.01;
				if(beta>0.99) beta=0.99;
				pen+= x[0]+x[3] > 0.0 ? 0.0 : exp(1.0-(x[0]+x[3]));
				pen+= x[3] > 0.0? 0.0 : exp(1.0-x[3]);
				pen+= x[2] > 0.0? 0.0 : exp(1.0-x[2]);
			}
			if(mode_==4) {
				nu = nu1+(nu2-nu1)/(t2-t1)*(optPillarTimes_[i]-t1);
			}
			if(mode_==5) {
				nu = nu2+(nu1-nu2)*exp(-lambda*(optPillarTimes_[i]-t1));
			}
			if(mode_==6) {
				nu = x[i]*x[i];
			}
			if(mode_==7) {
				nu = (x[0]+x[1]*(optPillarTimes_[i]-t1))*exp(-x[2]*(optPillarTimes_[i]-t1))+x[3];
				if(nu<0.0) nu=0.0;
				if(nu>maxNu_) nu=maxNu_;
				pen+= x[0]+x[3] > 0.0 ? 0.0 : exp(1.0-(x[0]+x[3]));
				pen+= x[3] > 0.0? 0.0 : exp(1.0-x[3]);
				pen+= x[2] > 0.0? 0.0 : exp(1.0-x[2]);
			}
			if(mode_==0 || mode_==1 || mode_==2 || mode_==3) {
				if(!useStdCube_) volCube_->setPillarBeta(i,undPillarIndex_,beta);
				else betaV.push_back(beta);
				pen += beta <=maxBeta_ ? 0.0 : exp(1.0+beta-maxBeta_);
				pen += beta >=minBeta_ ? 0.0 : exp(1.0+minBeta_-beta);
			}
			if(mode_==4 || mode_==5 || mode_==6 || mode_==7) {
				if(!useStdCube_) volCube_->setPillarNu(i,undPillarIndex_,nu);
				else nuV.push_back(nu);
				pen += nu <= maxNu_ ? 0.0 : exp(1.0+nu-maxNu_);
			}

#ifdef CMSHLOGGING
			fprintf(out,"    optPillar=%d nu=%1.12f, beta=%1.12f\n",i,nu,beta);
#endif

		}

		if(!useStdCube_) volCube_->recalibrate(noOptPillars_-1,undPillarIndex_,sabrRecal);
		else {
			// if opt pillars are skipped, fill up with flat values to the left
			for(int i=0;i<skipOptPillars_;i++) {
				if(mode_==0 || mode_==2) betaV[i]=betaV[skipOptPillars_];
				if(mode_==4 || mode_==6) nuV[i]=nuV[skipOptPillars_];
			}
			// if number of option pillars is lt vol cube option times, fill up with flat values to the right
			for(int i=noOptPillars_; i < volCube2_->optionTimes().size() ; i++) {
				if(mode_==0 || mode_==2) betaV.push_back(betaV[noOptPillars_-1]);
				if(mode_==4 || mode_==6) nuV.push_back(nuV[noOptPillars_-1]);
			}
			if(mode_==0 || mode_==2) volCube2_->recalibration(betaV,underlying_);
			if(mode_==4 || mode_==6) volCube2_->recalibrationNu(nuV,underlying_);
		}

		vector<double> mkt = helper_->marketMargins();
		for(int i=0;i<m_;i++) {
			double margin=helper_->margin(i);
			result[i] = weights_[i]*(margin - mkt[i]);// / mkt[optPillarIndex_];

#ifdef CMSHLOGGING
			fprintf(out,"     %f / %f\n",margin,mkt[i]);
#endif

		}

#ifdef CMSHLOGGING
		fprintf(out,"     %f\n",pen);
		fclose(out);
#endif

		result[m_] = pen;
		return result;

	}
double Reweight_Stop_to_TopChi0 (std::vector<IPHCTree::NTGenParticle> genParticles, double referenceTopPolarization, double requestedTopPolarization) 
{

    double weight = 1.;
    int nFoundStops = 0;

    unsigned int ngen = genParticles.size();

    for (unsigned int ig=0; ig<ngen; ++ig) 
    {
        const IPHCTree::NTGenParticle& gen = genParticles[ig];
        if (gen.motherIndex_<0) continue;
        if (abs(gen.id)>20) continue; // expect quarks or leptons from W decay

        // Navigate upwards in the stop->top->W->fermion decay chain
        const IPHCTree::NTGenParticle& genW = genParticles[gen.motherIndex_];
        if (genW.motherIndex_<0) continue;
        if (abs(genW.id)!=24) continue;
        const IPHCTree::NTGenParticle& genTop = genParticles[genW.motherIndex_];
        if (abs(genTop.id)!=6) continue;

        // We only care about the down-type fermion
        if (genTop.id*gen.id>0) continue;

        // We also need a stop
        if (genTop.motherIndex_<0) continue;
        const IPHCTree::NTGenParticle& genStop = genParticles[genTop.motherIndex_];
        if (abs(genStop.id)!=1000006) continue;

        // Move top and fermion to the stop center-of-mass frame
        TLorentzVector stop4 = genStop.p4;
        TVector3 betaV(-stop4.Px()/stop4.Energy(),-stop4.Py()/stop4.Energy(),-stop4.Pz()/stop4.Energy());

        TLorentzVector top4 = genTop.p4;
        top4.Boost(betaV);

        TLorentzVector ferm4;
        ferm4.SetPtEtaPhiE(gen.p4.Pt(), gen.p4.Eta(), gen.p4.Phi(), gen.p4.E());
        ferm4.Boost(betaV);

        // Do not reweight if by any reason top/fermion directions are undefined
        // This should be pathological if things are fine
        if (top4.P()<=0 || ferm4.P()<=0) {
            printf("Warning: particles at rest, no weight applied: ptop: %.3e, pf: %.3e\n", top4.P(), ferm4.P());
            continue; 
        }

        double costh = (top4.Px()*ferm4.Px()+top4.Py()*ferm4.Py()+top4.Pz()*ferm4.Pz())/top4.P()/ferm4.P();

        double weight_L = (top4.Energy()+top4.P())*(1-costh);
        double weight_R = (top4.Energy()-top4.P())*(1+costh);
        weight *= ((1+requestedTopPolarization)*weight_R+(1-requestedTopPolarization)*weight_L)/((1+referenceTopPolarization)*weight_R+(1-referenceTopPolarization)*weight_L);

        nFoundStops++;
    }

    if( nFoundStops!=2 ) cout << __FILE__ << " " << __LINE__ << " WARNING: found " << nFoundStops << " stops, should be 2." << endl;

    return weight;

};