/*
 * TpcCalibCluster.cxx
 *
 *  Created on: Apr 10, 2015
 *      Author: mberger
 */

#include <iostream>
#include <omp.h>

#include "TpcCalibCluster.h"
#include "TRandom.h"
#include "TNtupleD.h"
#include "TMatrixDSymEigen.h"
#include "TVectorD.h"
#include "TMatrixD.h"
#include "TMatrixDSym.h"
#include "TString.h"
#include "TMath.h"

#include "RooRealVar.h"
#include "RooMultiVarGaussian.h"
#include "RooDataSet.h"
#include "RooArgList.h"
#include "RooArgSet.h"
#include "RooFitResult.h"

#include "GFDetPlane.h"
#include "TpcClusterErrorScaler.h"

TpcCalibCluster::TpcCalibCluster()
:
fPos(0,0,0),
fPosUV(0,0,0),
fRefPos(0,0,0),
fRefPosPlane(0,0,0),
fev1Glob(0,0,0),
fev2Glob(0,0,0),
fResPlane(0,0,0),
fRes(0,0,0),
fev1(0,0,0),
fev2(0,0,0),
fParams(22,0),
fID(0),
fDafWeight(1),
fisFake(false),
fEigenVecIsSet(false),
fPlaneM(3,2),
fResPlaneD(2),
fsize(1,1,1),
fShapeCov(2),
fMeigenVec(2,2),
fMeigenVec_i(2,2)
{
	;
}

TpcCalibCluster::~TpcCalibCluster()
{;}

void TpcCalibCluster::setID(int i)
{
	fID=i;
}

void TpcCalibCluster::setPlane(TVector3 O, TVector3 U, TVector3 V)
{
	fPlane.set(O,U,V);
	fRefPosPlane=getPosOnPlane(fRefPos);
	//std::cout<<"make frefpos\n";
	//fRefPosPlane.Print();
	//fPlane.getU().Print();
	//fPlane.getV().Print();
	for (int i=0;i<3;i++)
	{
		fPlaneM[i][0]=fPlane.getU()[i];
		fPlaneM[i][1]=fPlane.getV()[i];
	}

}

void TpcCalibCluster::setPlaneON(TVector3 O,TVector3 N)
{
	fPlane.setON(O,N);
	fRefPosPlane=getPosOnPlane(fRefPos);
	for (int i=0;i<3;i++)
	{
		fPlaneM[i][0]=fPlane.getU()[i];
		fPlaneM[i][1]=fPlane.getV()[i];
	}
}

TVector3 TpcCalibCluster::getPulls(Bool_t fake)
{
	if (fake)
	{return produceFakePulls();}
	else
	{return calcPulls();}
}

Double_t TpcCalibCluster::getTheta()
{
	double theta=fPlane.getNormal().Theta();
	return theta;
}

TVector3 TpcCalibCluster::produceFakePulls()
{
	TVector3 retVec(0,0,0);
	//params: 0 offsetxy 1:diffusion 2:Amp 3: offsetZ
	//std::cout<<"producing fake pulls with parameters:"<<fParams[0]<<", "<<fParams[1]<<", "<<fParams[2]<<"\n";
	//retVec.SetX(fParams[0]+fPos.Z()*fParams[2]+gRandom->Gaus(0,fParams[1]));
	//retVec.SetY(fParams[0]+fPos.Z()*fParams[2]+gRandom->Gaus(0,fParams[1]));
	//retVec.SetZ(fParams[0]+fPos.Z()*fParams[2]+gRandom->Gaus(0,fParams[1]));
	//retVec.SetX(fParams[0]+fPos.Z()*fParams[2]+fParams[1]*fDigiPos[0].X());
	//retVec.SetY(fParams[0]+fPos.Z()*fParams[2]+fParams[1]*fDigiPos[0].X());
	//retVec.SetZ(fParams[0]+fPos.Z()*fParams[2]+fParams[1]*fDigiPos[0].Z());

	//TVector3 Res(fpos);
	//Res.SetX(0.003+0.001*sqrt(fPos.Z()));//+0.02*cos(2*getTheta()));
	//Res.SetY(0.003+0.001*sqrt(fPos.Z()));//+0.02*cos(2*getTheta()));
	//Res.SetZ(0.003+0.001*sqrt(fPos.Z()));//+0.02*cos(2*getTheta()));

	//retVec=TVector3(1000*Res);

	//fResPlane=getPosOnPlane(Res);

	TVector3 cog=getPosOnPlane(fPos);
	//std::cout<<"vectors:\n";
	//fPos.Print();
	//cog.Print();

	TMatrixDSym cov(2,getErrorCovPlane().GetMatrixArray());

	TMatrixDSymEigen eigenproblem(cov);
	TMatrixD eigenvects=eigenproblem.GetEigenVectors();
	TVectorD eigenvalues=eigenproblem.GetEigenValues();
	TVector3 ev1(eigenvects[0][0],eigenvects[1][0],0);
	TVector3 ev2(eigenvects[0][1],eigenvects[1][1],0);

	//return calcPulls(cog,ev1,ev2,eigenvalues[0],eigenvalues[1]);

	return retVec;
}

TVector3 TpcCalibCluster::calcPulls()
{
	TVector3 ret;
	TMatrixDSym cov(2,getErrorCovPlane().GetMatrixArray());

	TVector3 posOnPlane;
	posOnPlane=getPosOnPlane(fPos);

	//std::cout<<"cov on plane\n";
	//cov.Print();

	TMatrixDSymEigen eigenproblem(cov);
	TMatrixD eigenvects=eigenproblem.GetEigenVectors();
	TVectorD eigenvalues=eigenproblem.GetEigenValues();
	TVector3 ev1(eigenvects[0][0],eigenvects[1][0],0);
	TVector3 ev2(eigenvects[0][1],eigenvects[1][1],0);

	TVector3 retVec(0,0,0);
	TVector3 tmp_ev1Glob=ev1[0]*fPlane.getU()+ev1[1]*fPlane.getV();
	TVector3 tmp_ev2Glob=ev2[0]*fPlane.getU()+ev2[1]*fPlane.getV();
	tmp_ev1Glob.SetMag(1);
	tmp_ev2Glob.SetMag(1);
	fResPlane.SetXYZ(posOnPlane.X()-fRefPosPlane.X(),posOnPlane.Y()-fRefPosPlane.Y(),0.);
	fResPlaneD[0]=fResPlane.X();
	fResPlaneD[1]=fResPlane.Y();
	fRes=fPos-fRefPos;
	TVector3 resAxis((fResPlane*ev1)/ev1.Mag(),(fResPlane*ev2)/ev2.Mag(),0);

	fev1Glob=TVector3(tmp_ev1Glob*sqrt(eigenvalues[0]));
	fev2Glob=TVector3(tmp_ev2Glob*sqrt(eigenvalues[1]));
	retVec[0]=resAxis.X()/sqrt(eigenvalues[0]);
	retVec[1]=resAxis.Y()/sqrt(eigenvalues[1]);

	//std::cout<<"calib cluster axis errors: "<<sqrt(eigenvalues[0])<<" "<<sqrt(eigenvalues[1])<<"\n";
	//std::cout<<"calib cluster axis eigenval: "<<(eigenvalues[0])<<" "<<(eigenvalues[1])<<"\n";
	//std::cout<<"calib cluster eigenvects: ("<<ev1[0]<<","<<ev1[1]<<","<<ev1[2]<<") ("<<ev2[0]<<","<<ev2[1]<<","<<ev2[2]<<")\n";
	//std::cout<<"calib cluster axis: ("<<tmp_ev1Glob[0]<<","<<tmp_ev1Glob[1]<<","<<tmp_ev1Glob[2]<<") ("<<tmp_ev2Glob[0]<<","<<tmp_ev2Glob[1]<<","<<tmp_ev2Glob[2]<<")\n";
	//std::cout<<"calib cluster axis res: "<<resAxis.X()<<" "<<resAxis.Y()<<"\n";
	//std::cout<<"calib cluster pos: ("<<fPos.X()<<","<<fPos.Y()<<","<<fPos.Z()<<")\n";
	//std::cout<<"calib cluster track pos: ("<<fRefPos.X()<<","<<fRefPos.Y()<<","<<fRefPos.Z()<<")\n";
	//std::cout<<"calib cluster res: ("<<fRes.X()<<","<<fRes.Y()<<","<<fRes.Z()<<")\n";
	//std::cout<<"pulls\n";
	//retVec.Print();
	//std::cout<<"eigenvec:\n";
	//fev1Glob.Print();
	//fev2Glob.Print();
	return retVec;
	//return calcPullsUV(cog,hesse);
}

TVector3 TpcCalibCluster::calcPullsUV(TVector3 cog, TMatrixDSym cov)
{
	TVector3 retVec(0,0,0);

	fev1Glob=fPlane.getU();
	fev2Glob=fPlane.getV();
	fResPlane.SetXYZ(cog.X()-fRefPosPlane.X(),cog.Y()-fRefPosPlane.Y(),0.);

	//calculate errors in direction of U and V
	TVectorD U(2);
	TVectorD V(2);
	U[0]=1; U[1]=0;
	V[0]=0; V[1]=1;
	cov.Invert();
	double errU=sqrt(1./cov.Similarity(U));
	double errV=sqrt(1./cov.Similarity(V));

	retVec[0]=fResPlane.X()/errU;
	retVec[1]=fResPlane.Y()/errV;

	return retVec;
}

TMatrixDSym TpcCalibCluster::getErrorCov(bool project)
{
	TMatrixDSym errorCov(3);


	//fParams[1]=fParams[0];
	//fParams[4]=fParams[3];
	fParams[7]=fParams[6];
	fParams[10]=fParams[9];
	fParams[13]=fParams[12];
	fParams[16]=fParams[15];
	fParams[19]=fParams[18];


	fParams[14]=fParams[12];
	fParams[20]=fParams[18];

	double theta = fPlane.getNormal().Theta();
	double ct=TMath::Cos(fParams[1]*theta);
	double st=TMath::Sin(fParams[1]*theta);


	//first offset
	errorCov[0][0]=fParams[0];
	errorCov[1][1]=fParams[0];
	errorCov[2][2]=fParams[2];

	//diffusion driven spread of the electrons
	errorCov[0][0]+=fParams[3]*fPos.Z();//*fDt*fDt;
	errorCov[1][1]+=fParams[3]*fPos.Z();//*fDt*fDt;
	errorCov[2][2]+=fParams[5]*fPos.Z();//*fDl*fDl;

	//exponential with drift position decaying part due to finite pad size
	errorCov[0][0]+=fParams[9] *TMath::Exp(-fPos.Z()*fParams[6])*pow(st,2);
	errorCov[1][1]+=fParams[10]*TMath::Exp(-fPos.Z()*fParams[7])*pow(st,2);
	errorCov[2][2]+=fParams[11]*TMath::Exp(-fPos.Z()*fParams[8])*pow(st,2);

	//various angle models
	errorCov[0][0]+=fParams[12]*pow(ct,2);
	errorCov[1][1]+=fParams[13]*pow(ct,2);
	errorCov[2][2]+=fParams[14]*pow(st,2);

	errorCov[0][0]+=fParams[15]*pow(ct,4);
	errorCov[1][1]+=fParams[16]*pow(ct,4);
	errorCov[2][2]+=fParams[15]*pow(st,4);

	//mixed term diffusion and angle
	errorCov[0][0]+=fParams[18]*fPos.Z()*pow(ct,2);
	errorCov[1][1]+=fParams[19]*fPos.Z()*pow(ct,2);
	errorCov[2][2]+=fParams[20]*fPos.Z()*pow(st,2);

	if (project)
		errorCov=errorCov.SimilarityT(fPlaneM);

	return errorCov;
}

TMatrixDSym TpcCalibCluster::getErrorCovPlane()
{
	TMatrixDSym errorCov(2);

	TMatrixD S(2,2);

	double theta = fPlane.getNormal().Theta();
	double ct=TMath::Cos(theta);
	double st=TMath::Sin(theta);

	//first offset
	S[0][0]=fParams[0];
	S[1][1]=fParams[2];

	//diffusion driven spread of the electrons
	S[0][0]+=fParams[3]*fPos.Z();
	S[1][1]+=fParams[5]*fPos.Z();

	//exponential with drift position decaying part due to finite pad size
	S[0][0]+=fParams[9] *TMath::Exp(-fPos.Z()*fParams[6]);
	S[1][1]+=fParams[11]*TMath::Exp(-fPos.Z()*fParams[8]);

	S[0][0]+=fParams[12]*pow(ct,2);
	S[1][1]+=fParams[14]*pow(ct,2);

	//fit also the correlation factor
	//S[0][1]=S[1][0]=S[0][0]*S[1][1]*fParams[1];

	//std::cout<<"erroroCovPlane eigenvects:\n";
	//fMeigenVec.Print();
	//std::cout<<"errorCovPlane eigenval:\n";
	//S.Print();

	errorCov=TMatrixDSym(2,(fMeigenVec*S*fMeigenVec_i).GetMatrixArray());

	return errorCov;
}

double TpcCalibCluster::getResLL()
{
	double ll=0;

	TMatrixDSym cov(2,getErrorCovPlane().GetMatrixArray());

	TMatrixDSym cov_i(TMatrixDSym::kInverted,cov);

	//double norm=1./(pow(2*TMath::Pi(),2)*det );
	double expo=(cov_i.Similarity(fResPlaneD));

	//check if matrix is positive definite (inverse of posdef is posdef)
	if (expo<=0)
		return 0;

	ll= -0.5*log(fabs(cov.Determinant()))-0.5*(expo);

	ll*=fDafWeight;

	return ll;
}

double TpcCalibCluster::getResLL3D()
{
	double ll=0;

	TMatrixDSym cov(3);

	cov=getErrorCov(false);

	TMatrixDSym cov_i(TMatrixDSym::kInverted,cov);

	TVectorD res(3);
	res[0]=fRes.X();
	res[1]=fRes.Y();
	res[2]=fRes.Z();

	double det=cov.Determinant();

	double expo=(cov_i.Similarity(res));

	//check if matrix is positive definite (inverse of posdef is posdef)
	if (expo<=0)
		return 0;

	ll= -0.5*log(fabs(cov.Determinant()))-0.5*(expo);

	ll*=fDafWeight;

	return ll;
}

TVector3 TpcCalibCluster::getPosOnPlane(TVector3 pos)

{
	TVector3 planePos(0,0,0);
	pos-=fPlane.getO();
	planePos.SetX(pos*fPlane.getU()/fPlane.getU().Mag());
	planePos.SetY(pos*fPlane.getV()/fPlane.getV().Mag());
	return planePos;
}

void TpcCalibCluster::setShapeCov(TMatrixDSym m)
{
	fShapeCov=TMatrixDSym(2,m.GetMatrixArray());
	TMatrixDSymEigen tmpeig(fShapeCov);
	TMatrixD tmpvec=tmpeig.GetEigenVectors();
	fev1.SetXYZ(tmpvec[0][0],tmpvec[1][0],0);
	fev2.SetXYZ(tmpvec[0][1],tmpvec[1][1],0);

	fMeigenVec[0][0]=fev1.X(); fMeigenVec[0][1]=fev2.X();
	fMeigenVec[1][0]=fev1.Y(); fMeigenVec[1][1]=fev2.Y();

	fMeigenVec_i=TMatrixD(2,2,fMeigenVec.GetMatrixArray());
	fMeigenVec_i.Invert();

	fEigenVecIsSet=true;
}