//-----------------------------------------------------------
// File and Version Information:
// $Id$
//
// Description:
//     Class to calibrate cluster error parametrization
//
//
// Environment:
//      Software NOT developed for the PANDA Detector at FAIR.
//
// Author List:
//      Martin Berger      TUM            (original author)
//
//
//-----------------------------------------------------------
#include <map>
#include <vector>
#include <omp.h>
#include <sys/time.h>

#include "TObject.h"
#include "TH1.h"
#include "TH3D.h"
#include "TH2D.h"
#include "TH1D.h"
#include "TCanvas.h"
#include "TF1.h"
#include "TMath.h"

#include "TpcClusterErrorScaler.h"
#include "TpcClusterErrorCalib.h"
#include "BatClusterErrorModel.h"


TpcClusterErrorCalib::TpcClusterErrorCalib()
: 	fDataSetA(32,0.,75.,45,0.,TMath::Pi())

{
	fUseBat=kTRUE;
	fUseFake=kFALSE;

	fUseChi2Mean=kFALSE;
	fUseChi2Slope=kFALSE;
	fUseChi2Gaus=kFALSE;
	fUseChi2RMS=kFALSE;
	fUseLTest=kFALSE;

	fFixGausMean=kFALSE;
	fFitGaus=kTRUE;

	fVerbose=kFALSE;

	fUseZChi2=true;
	fUseThetaChi2=false;

	fFixDiffusion=-1;
	fFixAmp=-1;
	fFixOffsetXY=-1;
	fFixOffsetZ=-1;

	fPenalty0=kFALSE;
	fPenalty1=kFALSE;
	fPenalty2=kFALSE;

	fdrawGauss=kFALSE;

	fPdfName="bat.pdf";

	fcgauss=NULL;

	fMCMCPreRunMax=6000;
	fMCMCRunMax=3000;
	fstartBin=1;
	fstopBin=-1;
	fstartBinT=1;
	fstopBinT=-1;

	//	fDataSetA.setTheta(90,0,TMath::Pi()/2.);

	initHists();
}

void TpcClusterErrorCalib::addCalibCluster(TpcCalibCluster clus)
{
	fCalibClusterArray.push_back(clus);
	((TH3D*)fHists["hClPos"])->Fill(clus.getPos().X(),clus.getPos().Y(),clus.getPos().Z());
}



void TpcClusterErrorCalib::execute()
{


	if(fUseBat)
	{
		std::cout<<"Starting execution"<<std::endl;

		//first step, only look for offset decay and diffusion;
		//setting up model
		TString LogName=fPdfName;
		LogName.ReplaceAll(".pdf",".log");
		BCLog::OpenLog(LogName.Data(),BCLog::detail,BCLog::detail);
		BatClusterErrorModel *model= new BatClusterErrorModel(this);
		model->MCMCSetPrecision(BCEngineMCMC::kVeryHigh);
		model->MCMCSetNChains(3);
		model->MCMCSetNIterationsPreRunMax(fMCMCPreRunMax); //60000
		if (fMCMCPreRunMax<10000)
			model->MCMCSetNIterationsPreRunMin(fMCMCPreRunMax);

		model->MCMCSetNIterationsPreRunCheck(1000);
		model->MCMCSetNIterationsRun(fMCMCRunMax); //30000
		model->MCMCSetMultivariateProposalFunction(true);

		TString ChainName=fPdfName;
		ChainName.ReplaceAll(".pdf","_MCChain.root");
		model->WriteMarkovChain(ChainName.Data(),"RECREATE");

		//model->SetPriorDelta("OffsetX",0);			// 0
		model->SetPriorDelta("OffsetY",0);			// 1
		//model->SetPriorDelta("OffsetZ",0);			// 2

		//model->SetPriorDelta("DiffusionX",0);			// 3
		model->SetPriorDelta("DiffusionY",0);			// 4
		//model->SetPriorDelta("DiffusionZ",0);			// 5

		//model->SetPriorDelta("PadDecayX",0);			// 6
		model->SetPriorDelta("PadDecayY",0);			// 7
		//model->SetPriorDelta("PadDecayZ",0);			// 8

		//model->SetPriorDelta("PadDecayAmpX",0);		// 9
		model->SetPriorDelta("PadDecayAmpY",0);			//10
		//	model->SetPriorDelta("PadDecayAmpZ",0);		//11

		//model->SetPriorDelta("AngleX",0);				//12
		model->SetPriorDelta("AngleY",0);				//13
		//model->SetPriorDelta("AngleZ",0);				//14

		model->SetPriorDelta("AngleX2",0);				//15
		model->SetPriorDelta("AngleY2",0);				//16
		model->SetPriorDelta("AngleZ2",0);				//17

		model->SetPriorDelta("AngleXmixZ",0);			//18
		model->SetPriorDelta("AngleYmixZ",0);			//19
		model->SetPriorDelta("AngleZmixZ",0);			//20

		model->SetPriorDelta("FitContrib",0);			//21

		fstartBin=2;
		fstopBin=fDataSetA.getnZbins()-3;
		fstartBinT=2;
		fstopBinT=fDataSetA.getnThetabins()-3;
		model->MarginalizeAll(BCIntegrate::kMargMetropolis);
		model->MCMCCloseOutputFile();
		model->PrintAllMarginalized(fPdfName.Data(),2,2);
		TString resultsName=fPdfName;

		std::vector<double>tmp;

		//get best parameters and errors to set new limits for minuit
		model->SetOptimizationMethod(BCIntegrate::kOptMinuit);
		//use minuit
		model->FindMode();
		tmp=model->GetGlobalMode();

		//get best fit parameters from minuit
		for(int i=0;i<22;i++)
			fbest_pars.push_back(tmp[i]);

		tmp=model->GetBestFitParameterErrors();
		for(int i=0;i<22;i++)
			fbest_errs.push_back(tmp[i]);

		std::cout<<"best pars:"<<std::endl;
		for(int i=0;i<fbest_errs.size();i++)
			std::cout<<"parameter "<<i<<": "<<fbest_pars[i]<<std::endl;
		std::cout<<"final getChi2 to set parameter"<<std::endl;
		getChi2Res(fbest_pars);
		std::cout<<"TpcClusterCalib::execute done"<<std::endl;
	}
}

std::vector<double> TpcClusterErrorCalib::getBestParameter()
{
	return fbest_pars;
}

std::vector<double> TpcClusterErrorCalib::getBestParameterErrors()
{
	return fbest_errs;
}

void TpcClusterErrorCalib::initHists()
{
	TH2D* hP1vsZ=new TH2D("P1vsZ_before","Pulls A1 vs Z (before);Z (cm);Unit",75,0,75,500,-40,40);
	fHists["P1vsZ before"]=hP1vsZ;
	TH2D* hP1vsZ2=new TH2D("P1vsZ_after","Pulls A1 vs Z (after);Z (cm);Unit",75,0,75,500,-40,40);
	fHists["P1vsZ after"]=hP1vsZ2;
	TH2D* hP2vsZ=new TH2D("P2vsZ_before","Pulls A2 vs Z (before);Z (cm);Unit",75,0,75,500,-40,40);
	fHists["P2vsZ before"]=hP2vsZ;
	TH2D* hP2vsZ2=new TH2D("P2vsZ_after","Pulls A2 vs Z (after);Z (cm);Unit",75,0,75,500,-40,40);
	fHists["P2vsZ after"]=hP2vsZ2;
	TH1D* hTimeChi2=new TH1D("hTimeChi2","Time for calculating Chi2;t (ms);Counts",1000,0,0);
	fHists["TimeChi2"]=hTimeChi2;
	TH1D* hTimeData=new TH1D("hTimeData","Time to get Dataset;t (ms);Counts",1000,0,0);
	fHists["TimeData"]=hTimeData;

	TH2D* hThetaEv1=new TH2D("hThetaEv1_before","Theta vs Z of EV1 (before);Z (cm); Theta (Deg)",75,0,75,720,-360,360);
	fHists["ThetaEv1 before"]=hThetaEv1;
	TH2D* hThetaEv2=new TH2D("hThetaEv2_before","Theta vs Z of EV2 (before);Z (cm); Theta (Deg)",75,0,75,720,-360,360);
	fHists["ThetaEv2 before"]=hThetaEv2;
	TH2D* hThetaEv1_a=new TH2D("hThetaEv1_after","Theta vs Z of EV1 (after);Z (cm); Theta (Deg)",75,0,75,720,-360,360);
	fHists["ThetaEv1 after"]=hThetaEv1_a;
	TH2D* hThetaEv2_a=new TH2D("hThetaEv2_after","Theta vs Z of EV2 (after);Z (cm); Theta (Deg)",75,0,75,720,-360,360);
	fHists["ThetaEv2 after"]=hThetaEv2_a;


	TH2D* hR1vsZ=new TH2D("hR1vsZ","Residuals A1 vs Z (before);Z (cm); Residual (cm)",75,0,75,500,-0.5,0.5);
	fHists["R1vsZ before"]=hR1vsZ;
	TH2D* hR2vsZ=new TH2D("hR2vsZ","Residuals A2 vs Z (before);Z (cm); Residual (cm)",75,0,75,500,-0.5,0.5);
	fHists["R2vsZ before"]=hR2vsZ;
	TH2D* hR1vsZ2=new TH2D("hR1vsZ2","Residuals A1 vs Z (after);Z (cm); Residual (cm)",75,0,75,500,-0.5,0.5);
	fHists["R1vsZ after"]=hR1vsZ2;
	TH2D* hR2vsZ2=new TH2D("hR2vsZ2","Residuals A2 vs Z (after);Z (cm); Residual (cm)",75,0,75,500,-0.5,0.5);
	fHists["R2vsZ after"]=hR2vsZ2;

	TH2D* hE1vsZ=new TH2D("hE1vsZ_before","Error A1 vs Z (before);Z (cm); Error (cm)",75,0,75,500,0,0.5);
	fHists["E1vsZ before"]=hE1vsZ;
	TH2D* hE2vsZ=new TH2D("hE2vsZ_before","Error A2 vs Z (before);Z (cm); Error (cm)",75,0,75,500,0,0.5);
	fHists["E2vsZ before"]=hE2vsZ;
	TH2D* hE1vsZ2=new TH2D("hE1vsZ_after","Error A1 vs Z (after);Z (cm); Error (cm)",75,0,75,500,0,0.5);
	fHists["E1vsZ after"]=hE1vsZ2;
	TH2D* hE2vsZ2=new TH2D("hE2vsZ_after","Error A2 vs Z (after);Z (cm); Error (cm)",75,0,75,500,0,0.5);
	fHists["E2vsZ after"]=hE2vsZ2;

	TH2D* hE1vsT=new TH2D( "hE1vsT_before","Error A1 vs Theta (before);#theta (#circ); Error (cm)",90,0,180,500,0,0.5);
	TH2D* hE2vsT=new TH2D( "hE2vsT_before","Error A2 vs Theta (before);#theta (#circ); Error (cm)",90,0,180,500,0,0.5);
	TH2D* hE1vsT2=new TH2D("hE1vsT_after", "Error A1 vs Theta (after) ;#theta (#circ); Error (cm)",90,0,180,500,0,0.5);
	TH2D* hE2vsT2=new TH2D("hE2vsT_after", "Error A2 vs Theta (after) ;#theta (#circ); Error (cm)",90,0,180,500,0,0.5);
	fHists["E1vsT before"]=hE1vsT;
	fHists["E2vsT before"]=hE2vsT;
	fHists["E1vsT after"] =hE1vsT2;
	fHists["E2vsT after"] =hE2vsT2;

	TH2D* hP1vsTheta=new TH2D("hP1vsTheta_before","Pull A1 vs Theta;Theta (deg);Pull (Unit)",180,0,180,500,-40,40 );
	fHists["P1vsTheta before"]=hP1vsTheta;
	TH2D* hP2vsTheta=new TH2D("hP2vsTheta_before","Pull A2 vs Theta;Theta (deg);Pull (Unit)",180,0,180,500,-40,40 );
	fHists["P2vsTheta before"]=hP2vsTheta;
	TH2D* hP1vsTheta2=new TH2D("hP1vsTheta_after","Pull A1 vs Theta;Theta (deg);Pull (Unit)",180,0,180,500,-40,40 );
	fHists["P1vsTheta after"]=hP1vsTheta2;
	TH2D* hP2vsTheta2=new TH2D("hP2vsTheta_after","Pull A2 vs Theta;Theta (deg);Pull (Unit)",180,0,180,500,-40,40 );
	fHists["P2vsTheta after"]=hP2vsTheta2;

	TH3D* hClPos=new TH3D("hClPos","Cluster Position;X (cm);Y (cm);Z (cm)",30,-.5,.5,30,-.5,.5,75,0,75);
	fHists["hClPos"]=hClPos;
	//TH3D* hPosXzt=new TH3D("hPosXzt","Cluster Pos X vs Z and Theta;Z (cm);Theta (deg); posX (cm)",75,0,75,180,0,180,300,-15,15);
	//fHists["hPosXzt"]=hPosXzt;

}

void TpcClusterErrorCalib::getMyDataSet(std::vector<double> const &pars)
{
	unsigned int nThreads=1;
#pragma omp parallel
	{ nThreads = omp_get_num_threads();}

	std::vector<TpcClusterCalibDataset> pullset;

	fDataSetA.Reset("");

	for(int i=0;i<nThreads;i++)
	{
		pullset.push_back( TpcClusterCalibDataset(fDataSetA) );
	}

	int counter=-1;

	//fill dataset with pulls as a function of Z in parallel mode
	if(fUseThetaChi2 or fUseZChi2)
	{
#pragma omp parallel for num_threads(nThreads)
		for(int i=0;i<fCalibClusterArray.size();++i)
		{
			for(int j=0;j<12;j++)
			{
				fCalibClusterArray[i].setParam(j,pars[j]);
			}
			TVector3 pulls=fCalibClusterArray[i].getPulls(fUseFake);
			pullset[omp_get_thread_num()].Fill(1,fCalibClusterArray[i].getPos().Z(),pulls.X());
			pullset[omp_get_thread_num()].Fill(2,fCalibClusterArray[i].getPos().Z(),pulls.Y());

			pullset[omp_get_thread_num()].FillTheta(1,fCalibClusterArray[i].getTheta(),pulls.X());
			pullset[omp_get_thread_num()].FillTheta(2,fCalibClusterArray[i].getTheta(),pulls.Y());

		}
	}


	if(fUseZThetaChi2)
	{
#pragma omp parallel for num_threads(nThreads)
		for(int i=0;i<fCalibClusterArray.size();++i)
		{
			for(int j=0;j<12;j++)
			{
				fCalibClusterArray[i].setParam(j,pars[j]);
			}

			TVector3 pulls=fCalibClusterArray[i].getPulls(fUseFake);
			pullset[omp_get_thread_num()].FillZTheta(1,fCalibClusterArray[i].getPos().Z(),fCalibClusterArray[i].getTheta(),pulls.X());
			pullset[omp_get_thread_num()].FillZTheta(2,fCalibClusterArray[i].getPos().Z(),fCalibClusterArray[i].getTheta(),pulls.Y());
		}
	}


	//combine parallel created datasets
	if (fVerbose)
		std::cout<<"adding pullsets"<<std::endl;
	for (int i=0;i<nThreads;i++)
	{
		fDataSetA.Add(pullset[i]);
	}
	if (fVerbose)
		std::cout<<"pullsets added"<<std::endl;

}

double TpcClusterErrorCalib::getChi2p(double* pars)
{
	std::vector<double> fakepars;
	for (int i=0;i<13;i++)
		fakepars.push_back(pars[i]);
	return getChi2Res(fakepars);
}

double TpcClusterErrorCalib::getChi2p(double offXY, double diffXY, double offZ, double diffZ, double amp)
{
	std::vector<double> fakepars;
	fakepars.push_back(offXY);
	fakepars.push_back(diffXY);
	fakepars.push_back(offZ);
	fakepars.push_back(diffZ);
	fakepars.push_back(amp);
	return getChi2Res(fakepars);
}

double TpcClusterErrorCalib::getChi2p(bool fake)
{
	if (fake)
		return getChi2Res(fake_pars);
	else
		return getChi2Res(fCalibClusterArray[0].getParam());
}

double TpcClusterErrorCalib::getChi2(std::vector<double> const &pars)
{
	struct timeval t0;
	struct timeval t1;

	if (fstopBin==-1)
		fstopBin=fDataSetA.getnZbins()-1;
	if (fstopBinT==-1)
		fstopBinT=fDataSetA.getnThetabins()-1;
	time_t timer;
	timer=time(NULL);
	double t_getDataSet=0;
	gettimeofday(&t0, 0);

	//make dataset with the pulls
	getMyDataSet(pars);
	gettimeofday(&t1, 0);
	t_getDataSet=timedifference_usec(t0,t1);
	fHists["TimeData"]->Fill(t_getDataSet/1000);

	gettimeofday(&t0, 0);


	//prepare needed arrays
	unsigned int nThreads = 1 ;
#pragma omp parallel
	{ nThreads = omp_get_num_threads();}

	std::vector<std::vector<double> > chi2s_parallel;
	std::vector<std::vector<double> > observables_parallel;

	for(int i=0; i<nThreads;i++)
	{
		chi2s_parallel.push_back(std::vector<double>(12,0));
		observables_parallel.push_back(std::vector<double>(18,0));
	}

	double mean[2][fDataSetA.getnZbins()];
	double meanT[2][fDataSetA.getnThetabins()];
	double meanZT[2][fDataSetA.getnZbins()][fDataSetA.getnThetabins()];

	double meanErr[2][fDataSetA.getnZbins()];

	double rms[2][fDataSetA.getnZbins()];
	double rmsT[2][fDataSetA.getnThetabins()];
	double rmsZT[2][fDataSetA.getnZbins()][fDataSetA.getnThetabins()];

	double rmsErr[2][fDataSetA.getnZbins()];

	//needed for ltestGaus
	double rms2n[2][fDataSetA.getnZbins()];
	double rms2n0[2][fDataSetA.getnZbins()];
	double resSum[2][fDataSetA.getnZbins()];
	int nentries[fDataSetA.getnZbins()][2];

	double rms2nT[2][fDataSetA.getnThetabins()];
	double rms2n0T[2][fDataSetA.getnThetabins()];
	double resSumT[2][fDataSetA.getnThetabins()];
	int nentriesT[fDataSetA.getnThetabins()][2];


	double rms2nZT[2][fDataSetA.getnZbins()][fDataSetA.getnThetabins()];
	double rms2n0ZT[2][fDataSetA.getnZbins()][fDataSetA.getnThetabins()];
	double resSumZT[2][fDataSetA.getnZbins()][fDataSetA.getnThetabins()];
	int nentriesZT[fDataSetA.getnZbins()][fDataSetA.getnThetabins()][2];

	if(fUseZChi2)
	{
		//calculated all the variables needed for the log-likelihood ratio test (pull as function of z)
#pragma omp parallel for num_threads(nThreads)
		for(int i=fstartBin;i<fstopBin;i++)
		{
			mean[0][i]=mean[1][i]=rms2n[0][i]=rms2n[1][i]=rms2n0[0][i]=rms2n0[1][i]=resSum[0][i]=resSum[1][i]=rms[0][i]=rms[1][i]=0;
			nentries[i][0]=nentries[i][1]=0;
			if(fUseZChi2)
			{
				if(fUseChi2Mean or fUseChi2Slope)
				{
					meanErr[0][i]=fDataSetA.getMeanError(1,i-1,i+1);
					meanErr[1][i]=fDataSetA.getMeanError(2,i-1,i+1);
				}

				if(fUseChi2RMS)
				{
					rmsErr[0][i]=fDataSetA.getRMSError(1,i-1,i+1);
					rmsErr[1][i]=fDataSetA.getRMSError(2,i-1,i+1);
				}

				if(fUseLTest)
				{

					fDataSetA.getMeanRMS2N(mean[0][i],mean[1][i],rms2n[0][i],rms2n[1][i],rms2n0[0][i],rms2n0[1][i],resSum[0][i],resSum[1][i],nentries[i],i-1,i+1);
					if(nentries[i][0]!=0)
					{
						rms[0][i]=sqrt(rms2n[0][i]/nentries[i][0]);
						rms[1][i]=sqrt(rms2n[1][i]/nentries[i][1]);
					}
				}
			}
		}
	}
	if(fUseThetaChi2)
	{
		//calculated all the variables needed for the log-likelihood ratio test (pull as function of theta)
#pragma omp parallel for num_threads(nThreads)
		for(int i=fstartBinT;i<fstopBinT;i++)
		{
			meanT[0][i]=meanT[1][i]=rms2nT[0][i]=rms2nT[1][i]=rms2n0T[0][i]=rms2n0T[1][i]=resSumT[0][i]=resSumT[1][i]=rmsT[0][i]=rmsT[1][i]=0;
			nentriesT[0][0]=nentriesT[0][1]=0;
			if(fUseLTest)
			{
				fDataSetA.getMeanRMS2N_Theta(meanT[0][i],meanT[1][i],rms2nT[0][i],rms2nT[1][i],rms2n0T[0][i],rms2n0T[1][i],resSumT[0][i],resSumT[1][i],nentriesT[i],i-1,i+1);
				if(nentriesT[i][0]!=0)
				{
					rmsT[0][i]=sqrt(rms2nT[0][i]/nentriesT[i][0]);
					rmsT[1][i]=sqrt(rms2nT[1][i]/nentriesT[i][1]);
				}
			}
		}
	}
	if(fUseZThetaChi2)
	{
#pragma omp parallel for num_threads(nThreads)
		for(int i=fstartBin;i<fstopBin;i++)
		{
			for(int j=fstartBinT;j<fstopBinT;j++)
			{
				meanZT[0][i][j]=meanZT[1][i][j]=rms2nZT[0][i][j]=rms2nZT[1][i][j]=rms2n0ZT[0][i][j]=rms2n0ZT[1][i][j]=resSumZT[0][i][j]=resSumZT[1][i][j]=rmsZT[0][i][j]=rmsZT[1][i][j]=0;
				nentriesZT[i][j][0]=nentriesZT[i][j][1]=0;
				fDataSetA.getMeanRMS2N_ZTheta(meanZT[0][i][j],meanZT[1][i][j],rms2nZT[0][i][j],rms2nZT[1][i][j],rms2n0ZT[0][i][j],rms2n0ZT[1][i][j],resSumZT[0][i][j],resSumZT[1][i][j],nentriesZT[i][j],i-1,i+1,j-1,j+1);
				//std::cout<<"got crap: "<<meanZT[0][i][j]<<" "<<meanZT[1][i][j]<<" "<<rms2nZT[0][i][j]<<" "<<rms2nZT[1][i][j]<<" "<<rms2n0ZT[0][i][j]<<" "<<rms2n0ZT[1][i][j]<<" "<<resSumZT[0][i][j]<<" "<<resSumZT[1][i][j]<<" "<<nentriesZT[i][j][0]<<" "<<nentriesZT[i][j][1]<<std::endl;
				if(nentriesZT[i][j][0]!=0 and nentriesZT[i][j][1])
				{
					rmsZT[0][i][j]=sqrt(rms2nZT[0][i][j]/nentriesZT[i][j][0]);
					rmsZT[1][i][j]=sqrt(rms2nZT[1][i][j]/nentriesZT[i][j][1]);
				}
			}
		}
	}




	//calculate chi2 for pull as function of z
	if(fUseZChi2)
	{
#pragma omp parallel for num_threads(nThreads)
		for(int i=fstartBin; i<fstopBin;i++)
		{
			if(fUseChi2Mean)
			{
				if(meanErr[0][i]!=0)
					chi2s_parallel[omp_get_thread_num()][0]+=getChi2BinOff(mean[0][i],meanErr[0][i]);
				if(meanErr[1][i]!=0)
					chi2s_parallel[omp_get_thread_num()][1]+=getChi2BinOff(mean[1][i],meanErr[1][i]);
			}
			if(fUseChi2Slope and i<fDataSetA.getnZbins()-2)
			{
				if( meanErr[0][i]!=0 or meanErr[0][i+1]!=0)
					chi2s_parallel[omp_get_thread_num()][2]+=getChi2Slope(mean[0][i],mean[0][i+1],meanErr[0][i],meanErr[0][i+1]);

				if( meanErr[1][i+1]!=0 or meanErr[1][i+1]!=0)
					chi2s_parallel[omp_get_thread_num()][3]+=getChi2Slope(mean[1][i],mean[1][i+1],meanErr[1][i],meanErr[1][i+1]);
			}
			if(fUseChi2Gaus)
			{
				/*
			chi2s_parallel[omp_get_thread_num()][4]+=getChi2G(pullBins1[omp_get_thread_num()]);
			chi2s_parallel[omp_get_thread_num()][5]+=getChi2G(pullBins2[omp_get_thread_num()]);
				 */
			}
			if(fUseChi2RMS)
			{
				if(rmsErr[0][i]!=0)
					chi2s_parallel[omp_get_thread_num()][6]+=getChi2RMS(rms[0][i],rmsErr[0][i]);
				if(rms[1][i]!=0)
					chi2s_parallel[omp_get_thread_num()][7]+=getChi2RMS(rms[1][i],rmsErr[1][i]);
			}
			if(fUseLTest)
			{
				if(nentries[i][0]!=0)
				{
					chi2s_parallel[omp_get_thread_num()][4] += getLLGTest(rms2n[0][i],rms2n0[0][i],resSum[0][i],nentries[i][0],observables_parallel[omp_get_thread_num()],0);
				}
				else
				{
					chi2s_parallel[omp_get_thread_num()][4] += 10000;
					for(int iobs=0;iobs<3;iobs++)
						observables_parallel[omp_get_thread_num()][iobs]=10000;
				}

				if(nentries[i][1]!=0)
				{
					chi2s_parallel[omp_get_thread_num()][5] += getLLGTest(rms2n[1][i],rms2n0[1][i],resSum[0][i],nentries[i][1],observables_parallel[omp_get_thread_num()],3);

				}
				else
				{
					chi2s_parallel[omp_get_thread_num()][5] += 10000;
					for(int iobs=0;iobs<3;iobs++)
						observables_parallel[omp_get_thread_num()][iobs+3]=10000;
				}

				/*
			if(fdrawGauss)
			{
				fcgauss->cd();
				TH1D proj1=fDataSetA.ProjectionY(1,"some name",i-1,i+1);
				proj1.Draw();
				TH1D proj2=fDataSetA.ProjectionY(2,"some name",i-1,i+1);
				proj2.SetLineColor(2);
				proj2.Draw("same");
				fcgauss->Update();
				char a;
				a=std::cin.get();

			}
				 */
			}
		}
	}

	//calculate chi2 for pull as function of theta
	if (fUseThetaChi2)
	{
		//get chi2s for theta dependence
		if(fVerbose)
			std::cout<<"calculating pull theta chi2"<<std::endl;
#pragma omp parallel for num_threads(nThreads)

		for(int i=fstartBinT; i<fstopBinT;i++)
		{
			if(fUseLTest)
			{
				if(nentriesT[i][0]!=0)
				{
					chi2s_parallel[omp_get_thread_num()][8] += getLLGTest(rms2nT[0][i],rms2n0T[0][i],resSumT[0][i],nentriesT[i][0],observables_parallel[omp_get_thread_num()],6);
				}
				if(nentriesT[i][1]!=0)
				{
					chi2s_parallel[omp_get_thread_num()][9] += getLLGTest(rms2nT[1][i],rms2n0T[1][i],resSumT[0][i],nentriesT[i][1],observables_parallel[omp_get_thread_num()],9);
				}

#pragma omp critical
				{
					if(fdrawGauss)
					{
						std::cout<<"trying to draw bin "<<i-1<<" to "<<i+1<<std::endl;
						fcgauss->cd();
						TH1D proj1=TH1D(fDataSetA.ProjectionY(1,"h1",i-1,i+1));
						proj1.Draw();
						TH1D proj2=TH1D(fDataSetA.ProjectionY(2,"h2",i-1,i+1));
						proj2.SetLineColor(2);
						proj2.Draw("same");
						fcgauss->Update();
						char a;
						a=std::cin.get();
					}
				}

			}
		}
		if(fVerbose)
			std::cout<<"calculated pull theta chi2"<<std::endl;
	}

	//calculate chi2 for pull as function of theta
	if (fUseZThetaChi2)
	{
#pragma omp parallel for num_threads(nThreads)
		for(int i=fstartBin;i<fstopBin;i++)
			for(int j=fstartBinT; j<fstopBinT;j++)
			{
				if(fUseLTest)
				{
					if(nentriesZT[i][j][0]!=0)
					{
						//std::cout<<"making chi2 10:\n";
						//std::cout<<rms2nZT[0][i][j]<<" "<<rms2n0ZT[0][i][j]<<" "<<resSumZT[0][i][j]<<" "<<nentriesZT[i][j][0]<<std::endl;
						chi2s_parallel[omp_get_thread_num()][10] += getLLGTest(rms2nZT[0][i][j],rms2n0ZT[0][i][j],resSumZT[0][i][j],nentriesZT[i][j][0],observables_parallel[omp_get_thread_num()],12);
						//std::cout<<chi2s_parallel[omp_get_thread_num()][10]<<std::endl;
					}
					if(nentriesZT[i][j][1]!=0)
					{
						chi2s_parallel[omp_get_thread_num()][11] += getLLGTest(rms2nZT[1][i][j],rms2n0ZT[1][i][j],resSumZT[0][i][j],nentriesZT[i][j][1],observables_parallel[omp_get_thread_num()],15);
					}
				}
			}
		if(fVerbose)
			std::cout<<"calculated pull z theta chi2"<<std::endl;
	}


	double chi2=0;
	for(int j=0;j<12;j++)
		chi2s[j]=0;
	for(int j=0;j<18;j++)
	{
		fObservables[j]=0;
	}


	if(fVerbose)
		std::cout<<"adding up chi2s"<<std::endl;
	for (int k=0;k<nThreads;k++)
	{
		for(int j=0;j<12;j++)
		{
			if(chi2s_parallel[k][j]!=chi2s_parallel[k][j] or isinf(chi2s_parallel[k][j]))
			{
				chi2s_parallel[k][j]=10000;
			}
			//std::cout<<"chi2 par: "<<k<<" "<<j<<" "<<chi2s_parallel[k][j]<<std::endl;
			chi2s[j]+=chi2s_parallel[k][j];
			chi2+=chi2s_parallel[k][j];
		}
		for(int j=0;j<18;j++)
		{
			if(observables_parallel[k][j]!=observables_parallel[k][j] or isinf(observables_parallel[k][j]))
				observables_parallel[k][j]=10000;
			fObservables[j]+=observables_parallel[k][j];
		}
	}

	if(fVerbose)
		std::cout<<"calculating MEAN and RMS"<<std::endl;
	gettimeofday(&t1, 0);
	double t_getChi2=timedifference_usec(t0,t1);
	fHists["TimeChi2"]->Fill(t_getChi2/1000);

	//delete mean;
	fMeanRMS[0]=0;
	fMeanRMS[1]=0;
	fMeanRMS[2]=0;
	fMeanRMS[3]=0;

	fMeanRMS[4]=0;
	fMeanRMS[5]=0;
	fMeanRMS[6]=0;
	fMeanRMS[7]=0;

	fMeanRMS[8]=0;
	fMeanRMS[9]=0;
	fMeanRMS[10]=0;
	fMeanRMS[11]=0;

	if(fUseZChi2)
	{
		for(int i=fstartBin; i<fstopBin;i++)
		{
			fMeanRMS[0]+=mean[0][i];
			fMeanRMS[1]+=mean[1][i];
			fMeanRMS[2]+= rms[0][i];
			fMeanRMS[3]+= rms[1][i];
		}
		fMeanRMS[0]/=fstopBin-fstartBin-1;
		fMeanRMS[1]/=fstopBin-fstartBin-1;
		fMeanRMS[2]/=fstopBin-fstartBin-1;
		fMeanRMS[3]/=fstopBin-fstartBin-1;

		for(int i=fstartBin; i<fstopBin;i++)
		{
			fMeanRMS[8]+=(rms[0][i]-fMeanRMS[2])*(rms[0][i]-fMeanRMS[2]);
			fMeanRMS[9]+=(rms[1][i]-fMeanRMS[3])*(rms[1][i]-fMeanRMS[3]);
		}
		fMeanRMS[8] /=fstopBin-fstartBin-1;
		fMeanRMS[9] /=fstopBin-fstartBin-1;
		fMeanRMS[8] = sqrt(fMeanRMS[8]);
		fMeanRMS[9] = sqrt(fMeanRMS[9]);
	}

	if(fUseThetaChi2)
	{
		for(int i=fstartBinT; i<fstopBinT;i++)
		{
			fMeanRMS[4]+=meanT[0][i];
			fMeanRMS[5]+=meanT[1][i];
			fMeanRMS[6]+= rmsT[0][i];
			fMeanRMS[7]+= rmsT[1][i];
		}
		fMeanRMS[4]/=fstopBinT-fstartBinT-1;
		fMeanRMS[5]/=fstopBinT-fstartBinT-1;
		fMeanRMS[6]/=fstopBinT-fstartBinT-1;
		fMeanRMS[7]/=fstopBinT-fstartBinT-1;

		for(int i=fstartBinT; i<fstopBinT;i++)
		{
			fMeanRMS[10]+=(rmsT[0][i]-fMeanRMS[6])*(rmsT[0][i]-fMeanRMS[6]);
			fMeanRMS[11]+=(rmsT[1][i]-fMeanRMS[7])*(rmsT[1][i]-fMeanRMS[7]);
		}
		fMeanRMS[10]/=fstopBinT-fstartBinT-1;
		fMeanRMS[11]/=fstopBinT-fstartBinT-1;
		fMeanRMS[10]= sqrt(fMeanRMS[10]);
		fMeanRMS[11]= sqrt(fMeanRMS[11]);
	}

	if(fUseZThetaChi2)
	{
		for(int i=fstartBin; i<fstopBin;i++)
		{
			for(int j=fstartBinT; j<fstopBinT;j++)
			{
				fMeanRMS[0]+=meanZT[0][i][j];
				fMeanRMS[1]+=meanZT[1][i][j];
				fMeanRMS[2]+= rmsZT[0][i][j];
				fMeanRMS[3]+= rmsZT[1][i][j];
			}
		}
		fMeanRMS[0]/=fstopBin-fstartBin-1;
		fMeanRMS[1]/=fstopBin-fstartBin-1;
		fMeanRMS[2]/=fstopBin-fstartBin-1;
		fMeanRMS[3]/=fstopBin-fstartBin-1;

		for(int i=fstartBin; i<fstopBin;i++)
		{
			for(int j=fstartBinT; j<fstopBinT;j++)
			{
				fMeanRMS[8]+=(rmsZT[0][i][j]-fMeanRMS[2])*(rmsZT[0][i][j]-fMeanRMS[2]);
				fMeanRMS[9]+=(rmsZT[1][i][j]-fMeanRMS[3])*(rmsZT[1][i][j]-fMeanRMS[3]);
			}
		}
		fMeanRMS[8] /=fstopBin-fstartBin-1;
		fMeanRMS[9] /=fstopBin-fstartBin-1;
		fMeanRMS[8] = sqrt(fMeanRMS[8]);
		fMeanRMS[9] = sqrt(fMeanRMS[9]);

	}

	if (fVerbose)
		std::cout<<"done. found chi2="<<chi2<<std::endl;

	return chi2;
}

void TpcClusterErrorCalib::getMyDataSetRes()
{
	unsigned int nThreads=1;
#pragma omp parallel
	{ nThreads = omp_get_num_threads();}

	std::vector<TpcClusterCalibDataset> resset;

	fDataSetA.Reset("");

	for(int i=0;i<nThreads;i++)
	{
		resset.push_back( TpcClusterCalibDataset(fDataSetA) );
	}

	int counter=-1;

	//fill dataset with pulls as a function of Z in parallel mode
#pragma omp parallel for num_threads(nThreads)
	for(int i=0;i<fCalibClusterArray.size();++i)
	{
		TVector3 res=fCalibClusterArray[i].getResPlane();
		resset[omp_get_thread_num()].FillZTheta(1,fCalibClusterArray[i].getPos().Z(),fCalibClusterArray[i].getTheta(),res.X());
		resset[omp_get_thread_num()].FillZTheta(2,fCalibClusterArray[i].getPos().Z(),fCalibClusterArray[i].getTheta(),res.Y());
	}

	for (int i=0;i<nThreads;i++)
	{
		fDataSetA.Add(resset[i]);
	}

}

double TpcClusterErrorCalib::getChi2Res(std::vector<double> const &pars)
{
	double ll=0;

	unsigned int nThreads=1;
#pragma omp parallel
	{ nThreads = omp_get_num_threads();}

	std::vector<double> ll_parallel;
	for (int i=0;i<nThreads;i++)
		ll_parallel.push_back(0);

#pragma omp parallel for num_threads(nThreads)
	for(int icl=0;icl<fCalibClusterArray.size();++icl)
	{
		fCalibClusterArray[icl].setParams(pars);
		ll_parallel[omp_get_thread_num()]+=fCalibClusterArray[icl].getResLL();
		//ll_parallel[omp_get_thread_num()]+=fCalibClusterArray[icl].getResLL3D();
	}

	for(int i=0;i<nThreads;i++)
		ll+=ll_parallel[i];

	ll/=sqrt(fCalibClusterArray.size());

	return ll;
}

int TpcClusterErrorCalib::getBinsAbove(TH1D* hist, double thr)
{
	int nbins=0;
	for (int i=0;i<hist->GetNbinsX();i++)
	{
		if(hist->GetBinContent(i)>thr)
			nbins++;
	}

	return nbins;
}

double TpcClusterErrorCalib::getChi2G(TH1D* pullBin)
{


	double chi2=0;
	double height=0;

	if (fFitGaus)
	{
		TF1* gaus=new TF1("mygaus","gaus",-20,20);
		gaus->SetParameter(2,pullBin->GetRMS());
		if	(fFixGausMean)
		{
			gaus->FixParameter(1,0.);
		}
#pragma omp critical
		{pullBin->Fit(gaus,"QNORB","",-20,20);}
		chi2=gaus->GetChisquare();

		if(fdrawGauss)
		{
			TH1D* gausHist=(TH1D*)pullBin->Clone();
			gausHist->Reset();
			fcgauss->cd();
			pullBin->Draw();
			gaus->Draw("same");
			std::cout<<"fit const="<<gaus->GetParameter(0)<<" mean="<<gaus->GetParameter(1)<<" sig="<<gaus->GetParameter(2)<<" chi2="<<gaus->GetChisquare()<<std::endl;

			fcgauss->Update();
			char a;

			std::cout<<"chi2="<<chi2<<std::endl;
			std::cout<<"next\n";
			a=std::cin.get();
		}
		delete gaus;
	}
	else
	{
		//estimate the height of the distribution
		int meanBin=pullBin->GetXaxis()->FindBin(pullBin->GetMean());
		height=pullBin->GetBinContent(meanBin);
		int normalization=1;
		if (meanBin>0)
		{
			height=std::max(height,pullBin->GetBinContent(meanBin-1));
			normalization+=1;
		}
		if (meanBin<pullBin->GetNbinsX())
		{
			height=std::max(height,pullBin->GetBinContent(meanBin+1));
			normalization+=1;
		}

		//height/=normalization;

		//calculate chi2
		double mean=0;
		if(!fFixGausMean)
			mean=pullBin->GetMean();


		for(int i=0;i<pullBin->GetNbinsX();i++)
		{
			double x=pullBin->GetBinCenter(i);
			double expected=getGausExpected(x,height,mean);
			double diff=expected-pullBin->GetBinContent(i);
			chi2+=(diff*diff);
		}
		//chi2/=height;


		if(fdrawGauss)
		{
			TH1D* gausHist=(TH1D*)pullBin->Clone();
			gausHist->Reset();
			fcgauss->cd();
			pullBin->Draw();

			for(int i=0;i<gausHist->GetNbinsX();i++)
			{
				//std::cout<<"setting bin "<<i<<" at "<<gausHist->GetBinCenter(i)<<" to "<<getGausExpected(gausHist->GetBinCenter(i),height,pullBin->GetMean())<<"\n";
				gausHist->SetBinContent(i,getGausExpected(gausHist->GetBinCenter(i),height,mean));
			}
			std::cout<<"gaus height="<<height<<" mean="<<pullBin->GetMean()<<std::endl;
			gausHist->SetLineColor(2);
			gausHist->Draw("same");

			fcgauss->Update();
			char a;

			std::cout<<"chi2="<<chi2<<std::endl;
			std::cout<<"next\n";
			a=std::cin.get();
		}
	}

	return chi2;
}

double TpcClusterErrorCalib::getGausExpected(double x, double height, double mean)
{
	double ret=0;
	if (height==-1)
		height=0.39894228040143267794; //1/sqrt(2*Pi);
	ret=height*exp(- pow(x-mean,2)/2. );
	return ret;
}

double TpcClusterErrorCalib::getChi2RMS(double rms, double rmsErr)
{
	//std::cout<<"get chi2rms: "<<rms<<" "<<rmsErr<<" "<< ( (rms-1)*(rms-1) ) / (rmsErr*rmsErr)<<"\n";
	return ( (rms-1)*(rms-1) );// / (rmsErr*rmsErr);
}

double TpcClusterErrorCalib::getChi2BinOff(double mean, double meanErr)
{
	//std::cout<<"get chi2BinOff: "<<mean<<" "<<meanErr<<" "<<((mean-0)*(mean-0)) / (meanErr*meanErr)<<"\n";
	return ((mean-0)*(mean-0));// / (meanErr*meanErr);
}

double TpcClusterErrorCalib::getChi2Slope(double mean1, double mean2, double mean1Err, double mean2Err)
{
	double slope=mean2-mean1;
	double chi2=(slope-0)*(slope-0);
	//std::cout<<"get chi2slope: "<<mean1<<" "<<mean2<<" "<<mean1Err<<" "<<mean2Err<<" "<<chi2/ ( (mean1Err+mean2Err)*(mean1Err+mean2Err) )<<""<<std::endl;
	return chi2;// / ( (mean1Err+mean2Err)*(mean1Err+mean2Err) );
}

void TpcClusterErrorCalib::fillAngleHists(Int_t opt)
{
	//opt=0 before, opt=1 after marginalization
	TString nameadd="";
	if (opt==0)
		nameadd="before";
	else if(opt==1)
		nameadd="after";

	for (int i=0;i<fCalibClusterArray.size();i++)
	{
		if(i%1000==0)
		{
			std::cout<<"Filling Angle hists ("<<nameadd.Data()<<") "<<i<<" ("<<fCalibClusterArray.size()<<")"<<std::endl;
		}
		double zpos=fCalibClusterArray[i].getPos().Z();

		fHists["ThetaEv1 "+nameadd]->Fill(zpos,fCalibClusterArray[i].getEv1Glob().Theta()*TMath::RadToDeg());
		fHists["ThetaEv2 "+nameadd]->Fill(zpos,fCalibClusterArray[i].getEv2Glob().Theta()*TMath::RadToDeg());
	}
}

void TpcClusterErrorCalib::fillHPulls(Bool_t fit, Int_t opt)
{
	//opt=0 before, opt=1 after marginalization
	TString nameadd="";
	if (opt==0)
		nameadd="before";
	else if(opt==1)
		nameadd="after";

	for (int i=0;i<fCalibClusterArray.size();i++)
	{
		if(i%1000==0)
		{
			std::cout<<"Filling pull hists ("<<nameadd.Data()<<") "<<i<<" ("<<fCalibClusterArray.size()<<")"<<std::endl;
		}
		TVector3 pull=fCalibClusterArray[i].getPulls(fUseFake);
		TVector3 res=fCalibClusterArray[i].getResPlane();
		double zpos=fCalibClusterArray[i].getPos().Z();
		double theta=fCalibClusterArray[i].getTheta()*TMath::RadToDeg();

		fHists["P1vsZ "+nameadd]->Fill(zpos,pull.X());
		fHists["P2vsZ "+nameadd]->Fill(zpos,pull.Y());
		//std::cout<<"hist filling pulls: "<<pull.X()<<" "<<pull.Y()<<"\n";
		fHists["R1vsZ "+nameadd]->Fill(zpos,res.X());
		fHists["R2vsZ "+nameadd]->Fill(zpos,res.Y());
		fHists["E1vsZ "+nameadd]->Fill(zpos,fCalibClusterArray[i].getEv1Glob().Mag());
		fHists["E2vsZ "+nameadd]->Fill(zpos,fCalibClusterArray[i].getEv2Glob().Mag());
		fHists["E1vsT "+nameadd]->Fill(theta,fCalibClusterArray[i].getEv1Glob().Mag());
		fHists["E2vsT "+nameadd]->Fill(theta,fCalibClusterArray[i].getEv2Glob().Mag());
		fHists["P1vsTheta "+nameadd]->Fill(theta,pull.X());
		fHists["P2vsTheta "+nameadd]->Fill(theta,pull.Y());
	}
}

double TpcClusterErrorCalib::getLLG(std::vector<double>* v1,std::vector<double>* v2, std::vector<double>* v3)
{
	double ll=0;
	double n=0;
	double sum_residual=0;
	for(int i=0;i<v1->size();i++)
	{
		n++;
		sum_residual+=pow(v1->at(i)-0,2);
	}
	for(int i=0;i<v2->size();i++)
	{
		n++;
		sum_residual+=pow(v2->at(i)-0,2);
	}
	for(int i=0;i<v3->size();i++)
	{
		n++;
		sum_residual+=pow(v3->at(i)-0,2);
	}

	ll=-n/2. * 0.79817986835811504957 - sum_residual/2.; //0.79817986835811504957 = log(2*pi)

	return ll;
}

double TpcClusterErrorCalib::getLLGTest(std::vector<double>* v1,std::vector<double>* v2, std::vector<double>* v3, double mean, double rms)
{
	double n=0;

	double sum_meanTest=0;
	double sum_meanTest0=0;

	double sum_sigTest=0;
	double sum_sigTest0=0;

	for(int i=0;i<v1->size();i++)
	{
		n++;
		sum_meanTest+=pow(v1->at(i)-mean,2.);
		sum_meanTest0+=pow(v1->at(i),2.);
	}

	for(int i=0;i<v2->size();i++)
	{
		n++;
		sum_meanTest+=pow(v2->at(i)-mean,2.);
		sum_meanTest0+=pow(v2->at(i),2.);
	}

	for(int i=0;i<v3->size();i++)
	{
		n++;
		sum_meanTest+=pow(v3->at(i)-mean,2.);
		sum_meanTest0+=pow(v3->at(i),2.);
	}

	sum_sigTest=sqrt(sum_meanTest/n);

	double test_sigma = n*log(sum_sigTest)+n/2.*(1-pow(sum_sigTest,2)); //log(ilikelihood test for sigma)
	double test_mean  = n/2.*(log(sum_meanTest)-log(sum_meanTest0)); //log(likelihood test for mean)


	return test_sigma+test_mean;
}

double TpcClusterErrorCalib::getLLGTest(double sum_meanTest, double sum_meanTest0, double resSum, int n, std::vector<double> &observables,int obs_off)
{
	double sum_sigTest=sqrt(sum_meanTest/n);
	double test_mean=0;
	double test_sigma=10000;

	if(sum_sigTest!=0)
		test_sigma = (n*log(sum_sigTest)+n/2.*(1-pow(sum_sigTest,2) ) );  //log(likelihood test for sigma)
	if(sum_meanTest!=0)
		test_mean = (n/2.*( log(sum_meanTest)-log(sum_meanTest0) ) );    //log(likelihood test for mean)

	//double test_mean_error=n/2.*fabs(2*resSum/sum_meanTest * sum_sigTest); //derivative of test_mean * rms (rms=error of mean)
	//double test_sigma_error=fabs( (n/sum_sigTest - n*sum_sigTest) * sqrt( (sum_sigTest*sum_sigTest) / (2*n) )) ; //derivative of test_sigma * error of rms

	//std::cout<<"test mean:"<<test_mean<<" test sigma:"<<test_sigma<<"\n";
	observables[obs_off] =test_mean;
	observables[obs_off+1]=test_sigma;
	observables[obs_off+2]=n;

	test_sigma=fabs(test_sigma);
	test_mean =fabs(test_mean);

	return (test_sigma+test_mean)/n;///( sqrt( pow(test_mean_error,2) + pow(test_sigma_error,2) ) );
}

float TpcClusterErrorCalib::timedifference_usec(struct timeval t0, struct timeval t1)
{
	return (t1.tv_sec - t0.tv_sec) * 1000000.0f + (t1.tv_usec - t0.tv_usec) ;
}

double* TpcClusterErrorCalib::getMeanRMS()
{
	return fMeanRMS;
}