//-----------------------------------------------------------
// file and Version Information:
// $Id$
//
// Description:
//      Preliminary cluster to calculates Covariance, Errors, Position, ...
//      
//
// Environment:
//      Software developed for the FOPI-TPC Detector at GSI.
//
// Author List:
//          Unknown                TUM     (original author)
//          Martin Berger          TUM     (extensive changes)
//
//-----------------------------------------------------------
#include "TpcPrelimCluster.h"
#include <iostream>
#include <algorithm>

#include "TpcDigiMapper.h"
#include "TpcClusterErrorScaler.h"
#include "TpcPadPlane.h"
#include "TpcPadShapePolygon.h"
#include "TpcCluster.h"


#include "GFDetPlane.h"
#include "McId.h"
#include "McIdCollection.h"

#include "TMath.h"
#include "TMatrixD.h"
#include "TMatrixDEigen.h"
#include "TMatrixDSymEigen.h"
#include "TVectorD.h"
#include "TVector2.h"
#include "TNtupleD.h"
#include "TPolyMarker.h"
#include "TCanvas.h"
#include "TGraph2DErrors.h"
#include "TF2.h"
#include "TApplication.h"
#include "TSystem.h"
#include "TFitResult.h"
#include "TFitResultPtr.h"
#include "TRandom.h"

#include "RooRealVar.h"
#include "RooMultiVarGaussian.h"
#include "RooGaussian.h"
#include "RooAmpLandau.h"
#include "RooDataSet.h"
#include "RooArgSet.h"
#include "RooFitResult.h"
#include "RooGlobalFunc.h"
#include "RooProdPdf.h"
#include "RooConstVar.h"

#define debug 0
#define matrixDebug 0

Double_t gauss2d(Double_t* x, Double_t* par)
{
	double factor=2*TMath::Pi()*par[0]*par[1];
	factor*=TMath::Sqrt(1-par[2]*par[2]);
	factor=1./factor;

	double val1=(x[0]-par[3])*(x[0]-par[3]);
	val1/=par[0]*par[0];;
	double val2=(x[1]-par[4])*(x[1]-par[4]);
	val2/=par[1]*par[1];
	double val3=2*par[2]*(x[0]-par[3])*(x[1]-par[4]);
	val3/=par[0]*par[1];

	double exponent=-1./(2*(1-par[2]*par[2]));
	exponent*=(val1+val2-val3);

	double retval=par[5]*TMath::Exp(exponent);
	return retval;
}

TpcPrelimCluster::TpcPrelimCluster(TpcPadPlane* p, double t, int id, double G, double C) :
								famp(0.),
								famp2(0.),
								famp_weighted(0),
								famp2_weighted(0),
								fcogT(0.),
								fpadplane(p),
								ftimeslice(t),
								fid(id),
								fG(G),
								fC(C),
								fnUnsharedDigis(0),
								fphi(0),
								fdir(0,0,0),
								phiIsSet(kFALSE),
								fcov(3),
								fcovOnPlane(2),
								feigenVal(3),
								feigenVect(3,3),
								fpos(0,0,0),
								fposOnPlane(0,0,0),
								fcogpos(0,0,0),
								fCogUV(0,0,0),
								frecalcDigiPos(kFALSE),
								ffitGood(kFALSE),
								fDoPRF(kTRUE),
								fmaxDigiAmp(0),
								fcorrFactor(1.),
								feigenValOnPlane(0,0,0),
								fminSigma(0,0,0),
								//fchi2(0),
								//fndf(0)
								fminNll(1000),
								fshapeCov(3),
								fshapeCovPlane(2),
								funwShapeCov(3),
								funwShapeCovPlane(2)
{
	isGeomErr[0]=false;
	isGeomErr[1]=false;
	isGeomErr[2]=false;
}

TpcPrelimCluster::TpcPrelimCluster(TpcPadPlane* p, int id, double G, double C) :
								famp(0.),
								famp2(0.),
								famp_weighted(0),
								famp2_weighted(0),
								fcogT(0.),
								fpadplane(p),
								ftimeslice(0),
								fid(id),
								fG(G),
								fC(C),
								fnUnsharedDigis(0),
								fphi(0),
								fdir(0,0,0),
								phiIsSet(kFALSE),
								fcov(3),
								fcovOnPlane(2),
								feigenVal(3),
								feigenVect(3,3),
								fpos(0,0,0),
								fposOnPlane(0,0,0),
								fcogpos(0,0,0),
								fCogUV(0,0,0),
								frecalcDigiPos(kFALSE),
								ffitGood(kFALSE),
								fDoPRF(kTRUE),
								fmaxDigiAmp(0),
								fcorrFactor(1.),
								feigenValOnPlane(0,0,0),
								fminSigma(0,0,0),
								//fchi2(0),
								//fndf(0)
								fminNll(1000),
								fshapeCov(3),
								fshapeCovPlane(2),
								funwShapeCov(3),
								funwShapeCovPlane(2)
{
	isGeomErr[0]=false;
	isGeomErr[1]=false;
	isGeomErr[2]=false;
}

TpcPrelimCluster::~TpcPrelimCluster()
{

}

void TpcPrelimCluster::addHit(TpcDigi* digi, bool noXclust, double share, bool check)
{

	// check if digi is already there
	if (check && std::find(fdigis.begin(), fdigis.end(), digi) != fdigis.end()){
		fdigiShares[digi] += share;
	}
	else {
		fdigis.push_back(digi);
		fdigiShares[digi] = share;
	}

	// update amp and cogT
	double sharedAmp = digi->amp()*fdigiShares[digi];
	fcogT += sharedAmp * digi->t(); // not normalized!
	famp  += sharedAmp;
	famp2 += sharedAmp*sharedAmp;

	if (fdigiShares[digi] > 0.9999) ++fnUnsharedDigis;

	if (check) return; // we are in the second cluster merging pass - no updating of neighbours necessary!


	// update neighbours

	fpossiblePads.insert(digi->padId());

	TpcPad* pad = fpadplane->GetPad(digi->padId());
	for(unsigned int ineigh=0; ineigh<pad->nNeighbours(); ++ineigh){
		if(noXclust){
			TpcPad* neigh = fpadplane->GetPad( pad->getNeighbour(ineigh) );
			if(fabs(pad->x()-neigh->x())>0.01) continue; //gt 0.1mm
		}

		if (share > 0.9999)
		{
			fpossiblePads.insert(pad->getNeighbour(ineigh));
		}
		else
		{
			fpossiblePadsShared.insert(pad->getNeighbour(ineigh));
		}
	}

}

void TpcPrelimCluster::addHit(TpcDigi* digi, GFDetPlane plane, double share)
{
	// check if digi is already there
	if (std::find(fdigis.begin(), fdigis.end(), digi) != fdigis.end()){
		fdigiShares[digi] += share;
	}
	else {
		fdigis.push_back(digi);
		fdetPlanes.push_back(plane);
		fdigiShares[digi] = share;
	}

	famp  += digi->amp() * share;
	famp2 += digi->amp()*share * digi->amp()*share;
	fcogT += digi->amp() * share * digi->t();
	//fdigis.push_back(digi);
	//fdetPlanes.push_back(plane);
	//fdigiShares[digi] = share;
}

void TpcPrelimCluster::calcCog()
{
	// loop over digis to calculate cog
	McIdCollection mcid;
	unsigned int ndigis=fdigis.size();
	TpcDigi* adigi;
	TVector3 thispos=TVector3(0,0,0);
	fpos.SetXYZ(0,0,0);
	famp=0;
	famp2=0;
	fmaxDigiAmp=0;
	for(unsigned int id=0;id<ndigis;++id)
	{
		adigi = fdigis[id];
		if (std::find(fpadids.begin(), fpadids.end(),adigi->padId())==fpadids.end())
		{
			fpadids.push_back(adigi->padId());
		}
		if (std::find(ftbins.begin(), ftbins.end(),adigi->t())==ftbins.end())
		{
			ftbins.push_back(adigi->t());
		}

		double a = (double)adigi->amp()*fdigiShares[adigi];
		mcid.AddIDCollection(adigi->mcId(),a);
		try
		{
			TpcDigiMapper::getInstance()->map(adigi,thispos);
		}
		catch(const std::exception& ex)
		{
			std::cout<<ex.what()<<std::endl;
		}
		fpos += a*thispos;
		famp += a;
		famp2+= a*a;
		fmaxDigiAmp=std::max(a,fmaxDigiAmp);
		//std::cout<<"prelim digipos: "<<thispos.X()<<" "<<thispos.Y()<<" "<<thispos.Z()<<"\n";
	}
	fpos *= 1./famp;
	//std::cout<<"prelim clusterpos: "<<fpos.X()<<" "<<fpos.Y()<<" "<<fpos.Z()<<"\n";
	fcogpos=TVector3(fpos);
	fmcidCol = mcid;
}

bool TpcPrelimCluster::isInTimeWindow(const TpcDigi* const digi){
	if( fabs(digi->t() - fcogT/famp) <= ftimeslice ) // fcogT has to be normalized with famp
		return true;
	return false;
}

int TpcPrelimCluster::isInCluster(const TpcDigi* const digi) {
	if(!isInTimeWindow(digi))
		return 0; // not a neighbour
	int padID = digi->padId();
	if(fpossiblePads.find(padID) == fpossiblePads.end()) {  // not a neighbour to any unshared digi
		if(fpossiblePadsShared.find(padID) == fpossiblePadsShared.end()) return 0;  // not a neighbour to any shared digi
		return 2;   // neighbour to a shared digi
	}
	return 1; // neighbour to an unshared digi
}

void TpcPrelimCluster::addSharedCluster(TpcDigi* d, TpcPrelimCluster* c)
{
	if(fSharedClusters[d].find(c)!=fSharedClusters[d].end())
		fSharedClusters[d].insert(c);
}

TpcCluster* TpcPrelimCluster::convTpcCluster(unsigned int sectorId, double zJitter, bool saveRaw, bool usePlane)
{
	calcCog();

	if (usePlane)
	{
		if(TpcClusterErrorScaler::getInstance()->getScaleMode()>=60)
			calcErrorPlaneParam(zJitter);
		else
			calcErrorPlane(zJitter);
	}
	else
	{
		calcError(zJitter);
	}

	TpcCluster* c = new TpcCluster(fpos,ferr,famp,fid);
	c->SetMcId(fmcidCol);
	c->SetSigT(ferrT);
	c->SetPhi(fphi);
	c->SetMidPlane(fmidPlane);

	c->SetCov(TMatrixD(fcov));
	c->SetShapeCov(fshapeCov);
	c->SetUnwShapeCov(funwShapeCov);
	c->SetCovOnPlane(fcovOnPlane);
	c->SetShapeCovPlane(fshapeCovPlane);
	c->SetUnwShapeCovPlane(funwShapeCovPlane);

	//always store digis inside the cluster, only index and weight is copied
	//std::cout<<"there are "<<fdigis.size()<<"digis in prelim cluster\n";
	for(unsigned int i=0;i<fdigis.size();++i){
		if(fdigis[i]->isSaturated()){
			c->setSaturation(true);
		}
		c->addDigi(fdigis[i], fdigiShares[fdigis[i]]);
	}

	c->SetSector(sectorId);
	//TMatrixD fcov2=TMatrixD(fcov);
	//c->SetCov(fcov2);

	//sort eigenvectors and axis error by z-component
	TVectorD a1=TMatrixDColumn(feigenVect,1);
	TVector3 Axis1(a1[0],a1[1],a1[2]);
	TVectorD a2=TMatrixDColumn(feigenVect,2);
	TVector3 Axis2(a2[0],a2[1],a2[2]);

	/*
	if(fabs(Axis1.Z())>fabs(Axis2.Z()))
	{
		ferrA=TVector3(ferrA[0],ferrA[2],ferrA[1]);
		bool tmp=isGeomErr[1];isGeomErr[1]=isGeomErr[2];isGeomErr[2]=tmp;
		feigenVect[0][1]=Axis2[0];feigenVect[1][1]=Axis2[1];feigenVect[2][1]=Axis2[2];
		feigenVect[0][2]=Axis1[0];feigenVect[1][2]=Axis1[1];feigenVect[2][2]=Axis1[2];
		feigenValOnPlane=TVector3(0,feigenValOnPlane[2],feigenValOnPlane[1]);
	}
	 */

	c->SetSigA(ferrA);
	c->SetIsGeomErr(isGeomErr[0],isGeomErr[1],isGeomErr[2]);
	c->SetEigenVect(feigenVect);
	c->SetEigenValOnPlane(feigenValOnPlane);

	c->SetFitGood(ffitGood);
	c->SetMinSigma(fminSigma);
	c->SetCogPos(fcogpos);
	c->SetMinNll(fminNll);
	c->SetPrfWeights(fresponseweights);

	return c;
}

void TpcPrelimCluster::calcErrorPlaneParam(double zJitter)
{
	TVector3 planeU=fmidPlane.getU();
	TVector3 planeV=fmidPlane.getV();
	TVector3 planeW=fmidPlane.getNormal();
	TVector3 planeO=fmidPlane.getO();

	//create UVplane Matrix
	TMatrixD UVplane(3,2);
	for (int i=0;i<3;i++)
	{
		UVplane[i][0]=planeU[i];
		UVplane[i][1]=planeV[i];
	}

	fresponseweights.assign(fdigis.size(),1.);

	//find center of gravity and fill digisOnPlaneTuple
	TNtupleD* digisOnPlaneTuple=new TNtupleD("UVdigipos","","u:v:amp");
	getCogOnPlane(digisOnPlaneTuple);

	//get shapecov from digi distribution
	getShapeCovFromDigis(UVplane);
	TMatrixDSym covOnPlaneSym=TMatrixDSym(2,fshapeCovPlane.GetMatrixArray());

	//calculate eigenvectors on plane
	TMatrixDSymEigen eigenCovOnPlane(covOnPlaneSym);
	TVectorD eigenValuesOnPlane=eigenCovOnPlane.GetEigenValues();
	TMatrixD eigenVectOnPlane=eigenCovOnPlane.GetEigenVectors();

	//calculate cluster size
	TVector3 size=calcClusterSize(digisOnPlaneTuple,TVector3(eigenVectOnPlane[0][0],eigenVectOnPlane[1][0],0),TVector3(eigenVectOnPlane[0][1],eigenVectOnPlane[1][1],0));

	//generate pad shape cov (use only dy. in our case this is the diameter of the hexagonal pads)
	double dx, dy;
	TpcDigiMapper::getInstance()->padsize(fdigis[0]->padId(),dx,dy);
	double wx,wy;
	TpcClusterErrorScaler::getInstance()->getPRFWeight(wx,wy);
	wx*=2;wy*=2;
	TMatrixDSym padCov=TMatrixDSym(3);
	padCov[0][0]=wx*wx; padCov[0][1]=0;    padCov[0][2]=0;
	padCov[1][0]=0;     padCov[1][1]=wy*wy;padCov[1][2]=0;
	padCov[2][0]=0;     padCov[2][1]=0;    padCov[2][2]=zJitter*zJitter;

	//calculate pad shape on detplane
	TMatrixDSym padCovOnPlane(2);
	padCovOnPlane=padCov.SimilarityT(UVplane);

	//check if cluster size along eigenvector is smaller than projection of padshapecov onto eigenvector
	bool redoCov[2];
	isGeomErr[0]=true; //always true since error along track is assigned by hand
	redoCov[0]=false;redoCov[1]=false;
	bool usePRF=false;
	for (int i=0 ;i<2; i++)
	{
		Double_t arr[2];
		arr[0]=eigenVectOnPlane[0][i];
		arr[1]=eigenVectOnPlane[1][i];
		TVectorD vec(2,arr);
		//get projection of covonplane with shape cov eigenvector to determine if to small to fit
		if(matrixDebug)
			std::cout<<"get minsigma from geomCovOnPlane\n";
		double minShapeSigma=padCovOnPlane.Similarity(vec);
		fminSigma[i+1]=minShapeSigma;
		//use sqrt, since pad geometry goes quadratic into matrix, half since dy is diameter and we want to check here for radius
		if (size[i]<=TMath::Sqrt(minShapeSigma)/2.)
		{
			redoCov[i]=true;
			isGeomErr[i+1]=true;
		}
		//use sqrt, since pad geometry goes quadratic into matrix, here we want to check for the diameter (with additional offset)
		if(size[i]<TMath::Sqrt(minShapeSigma)*1.05)
			usePRF=true;
	}
	//if above was the case, recalculate the cov (from fit) later, using the geometrical error

	//also if above is the case, use the padresponse correction
	//get the weightings for the amplitudes from padresponse function and recalculate cog
	if(usePRF and fDoPRF)
	{
		getDigiWeights();
		//find center of gravity and fill digisOnPlaneTuple
		delete digisOnPlaneTuple;

		digisOnPlaneTuple=new TNtupleD("UVdigipos","","u:v:amp");
		getCogOnPlane(digisOnPlaneTuple);

		getShapeCovFromDigis(UVplane);
		covOnPlaneSym=TMatrixDSym(2,fshapeCovPlane.GetMatrixArray());
	}
	//fitting the digi distribution
	TMatrixDSym fitCov(2);
	TVector2 shift;
	fitCov=fitDigis2D(digisOnPlaneTuple,covOnPlaneSym,shift);


	if(!ffitGood)
	{
		fitCov=calcHesse();
		fpos=fCogUV.X()*planeU+fCogUV.Y()*planeV+planeO;
		fposOnPlane=TVector3(fCogUV);
	}
	else
	{
		fpos=planeO+shift.X()*planeU+shift.Y()*planeV;
		fposOnPlane=TVector3(shift.X(),shift.Y(),0);
	}


	//get parameterized errorCov from ClusterErrorScaler
	TMatrixDSym errorCov(3);
	TMatrixDSym errorCovPlane(2);errorCovPlane=TpcClusterErrorScaler::getInstance()->getErrorCovOnPlane(-1,UVplane,fshapeCovPlane,fcogpos.Z());
	//errorCovPlane.SimilarityT(UVplane);


	double errT=( fdir.Mag2()/12. );

	if(errorCov[0][0]!=0 and errorCov[1][1]!=0 and errorCov[2][2]!=0)
	{
		//get error in direction along the track by projecting param cov on direction vector
		TVectorD dirD(3); dirD[0]=planeW[0];dirD[1]=planeW[1];dirD[2]=planeW[2];
		errT=errorCov.Similarity(dirD);
	}


	//scale fitcov down and add error cov
	fitCov=fitCov * ( TpcClusterErrorScaler::getInstance()->getFitFractionPar() ) + errorCovPlane;

	//eigenvalues and eigenvectors of fitCov
	TMatrixDSymEigen eigen(fitCov);
	TVectorD fiteigenValues=eigen.GetEigenValues();
	TMatrixD fiteigenVect=eigen.GetEigenVectors();

	//if a size along a axis was to small, recalculate the error now, by using the geometrical error.
	TVector3 eigenVects[3];
	eigenVects[1]=fiteigenVect[0][0]*planeU+fiteigenVect[1][0]*planeV;
	eigenVects[2]=fiteigenVect[0][1]*planeU+fiteigenVect[1][1]*planeV;
	eigenVects[0]=eigenVects[1].Cross(eigenVects[2]);
	TVector3 eigenVal;

	eigenVal.SetX(errT);

	for (int i=0 ;i<2; i++)
	{
		//if(redoCov[i])
		//	eigenVal[i+1]=fminSigma[i+1]/12;
		//else
			eigenVal[i+1]=( fiteigenValues[i] );
	}


	ferrA.SetXYZ(sqrt(eigenVal[0]),sqrt(eigenVal[1]),sqrt(eigenVal[2]));

	//construct eigenvector and eigenvalue matrix
	TMatrixDSym S(3);
	for (int row=0;row<3;row++)
	{
		feigenVal[row]=eigenVal[row];
		for (int col=0;col<3;col++)
		{
			feigenVect[row][col]=eigenVects[col][row];
			S[row][col]=0;
			if(row==col)
			{
				S[row][col]=eigenVal[row];
			}
		}
	}

	TMatrixD eigenVect_i=TMatrixD(TMatrixD::kInverted,feigenVect);
	fcov=TMatrixDSym(3, ( feigenVect*S*eigenVect_i ).GetMatrixArray() );

	TVectorD ax(3);
	ax[0]=1;ax[1]=0;ax[2]=0;
	double errX=fcov.Similarity(ax);
	ax[0]=0;ax[1]=1;ax[2]=0;
	double errY=fcov.Similarity(ax);
	ax[0]=0;ax[1]=0;ax[2]=1;
	double errZ=fcov.Similarity(ax);

	ferr.SetXYZ(sqrt(errX),sqrt(errY),sqrt(errZ));

	fcovOnPlane=fitCov;

	delete digisOnPlaneTuple;
}

void TpcPrelimCluster::calcErrorPlane(double zJitter)
{
	//geometrical constants
	double dx, dy;
	TpcDigiMapper::getInstance()->padsize(fdigis[0]->padId(),dx,dy);
	double Dl = TpcDigiMapper::getInstance()->getGas()->Dl();
	double Dt = TpcDigiMapper::getInstance()->getGas()->Dt();
	double mean_ftLength=0;

	//collect digipositions in detplane coordinates
	unsigned int ndigis=fdigis.size();

	//digisOnPlaneTuple->SetCircular(ndigis+5);
	TNtupleD digisOnAxisTuple("AxDigiPos","","x:amp");
	TVector3 thispos;
	TpcDigi* adigi;
	//TVector3 cogUV(0,0,0);

	TMatrixD covOnPlane1(2,2);

	TVector3 planeU;
	TVector3 planeV;
	TVector3 planeO;

	planeU=(TVector3)fmidPlane.getU();
	planeV=(TVector3)fmidPlane.getV();
	planeO=(TVector3)fmidPlane.getO();


	//-------------------------------------------------------------------------------------------------

	//create UVplane Matrix
	TMatrixD UVplane(3,2);
	for (int i=0;i<3;i++)
	{
		UVplane[i][0]=planeU[i];
		UVplane[i][1]=planeV[i];
	}

	//-------------------------------------------------------------------------------------------------

	//get zjitter of digi on plane
	TVector3 zjitterOnPlane(0,0,0);
	TVector3 digizJitter(0,0,zJitter);
	zjitterOnPlane=digizJitter - ( (digizJitter*fmidPlane.getNormal()) / fmidPlane.getNormal().Mag2() ) *fmidPlane.getNormal();
	zjitterOnPlane.SetXYZ(zjitterOnPlane*planeU/planeU.Mag(),zjitterOnPlane*planeV/planeV.Mag(),0);


	//get binsize of a digi on plane
	TVector3 digiBin(0,0,0);
	TMatrixDSym digiBinCov(3);
	TMatrixDSym digiBinCovOnPlane(2);
	digiBinCov[0][1]=digiBinCov[0][2]=digiBinCov[1][0]=digiBinCov[2][0]=digiBinCov[1][2]=digiBinCov[2][1]=0;
	digiBinCov[0][0]=1./(dx);
	digiBinCov[1][1]=1./(dy);
	digiBinCov[2][2]=1./(zJitter);
	digiBinCovOnPlane=digiBinCov.SimilarityT(UVplane);
	if(digiBinCovOnPlane[0][0]!=0)
		digiBin.SetX(1./digiBinCovOnPlane[0][0]);
	if(digiBinCovOnPlane[1][1]!=0)
		digiBin.SetY(1./digiBinCovOnPlane[1][1]);

	//-------------------------------------------------------------------------------------------------


	//first create cov representing geometrical resolution in xyz
	TMatrixD geomCov(3,3);
	geomCov[0][1]=geomCov[0][2]=geomCov[1][0]=geomCov[1][2]=geomCov[2][0]=geomCov[2][1]=0;
	//geomCov[0][0]=dx*dx/(8*TMath::Log(10));
	//geomCov[1][1]=dy*dy/(8*TMath::Log(10));
	geomCov[0][0]=dx*dx/12;
	geomCov[1][1]=dy*dy/12;
	geomCov[2][2]=zJitter*zJitter/12;//(8*TMath::Log(10));


	//-------------------------------------------------------------------------------------------------

	//create geometric shape cov
	TMatrixD geomShapeCov(3,3);
	TpcPadShapePolygon* shape;
	shape=(TpcPadShapePolygon*)fpadplane->GetPad(0)->shape();
	double xBound,yBound;
	for (int ijitter=0;ijitter<2;ijitter++)
		for (int ibound=0;ibound<fpadplane->GetPad(0)->GetNBoundaryPoints();ibound++)
		{
			shape->GetBoundaryPoint(ibound,xBound,yBound);
			TMatrixD c(3,1);
			c[0][0]=xBound;
			c[1][0]=yBound;
			c[2][0]=(zJitter*ijitter)-zJitter/2.;
			TMatrixD acov(c , TMatrixD::kMult , TMatrixD(TMatrixD::kTransposed,c) );
			geomShapeCov+=acov;
		}
	geomShapeCov*=1./(2*fpadplane->GetPad(0)->GetNBoundaryPoints());
	TMatrixD geomShapeCovInv=TMatrixD(geomShapeCov);
	geomShapeCovInv.Invert();

	//re-weight geomCov entries
	if(frecalcDigiPos)
	{
		double wx=(fpadplane->GetPad(0)->GetWeight(0.99,1.,0))+1;
		double wy=(fpadplane->GetPad(0)->GetWeight(0.99,0,1.))+1;
		geomCov[0][0]=wx*2*wx*2/12; // TODO this is wrong!!
		geomCov[1][1]=wy*2*wy*2/12; // TODO this is wrong !!
	}

	//invert matrix to calculate CUT
	TMatrixD geomCovInv(3,3);
	geomCovInv[0][1]=geomCovInv[0][2]=geomCovInv[1][0]=geomCovInv[1][2]=geomCovInv[2][0]=geomCovInv[2][1]=0;
	geomCovInv[0][0]=1./geomCov[0][0];
	geomCovInv[1][1]=1./geomCov[1][1];
	geomCovInv[2][2]=1./geomCov[2][2];

	//cut geomcov with uv plane to get correct geometrical error representation
	TMatrixD geomCovOnPlane=TMatrixD( TMatrixD(TMatrixD::kTransposed,UVplane), TMatrixD::kMult, TMatrixD(geomCovInv, TMatrixD::kMult, UVplane) );
	TMatrixD geomShapeCovOnPlane=TMatrixD(TMatrixD(TMatrixD::kTransposed,UVplane),TMatrixD::kMult,TMatrixD(geomShapeCovInv,TMatrixD::kMult,UVplane) );

	//-------------------------------------------------------------------------------------------------

	//get the weightings for the amplitudes from padresponse function
	//std::vector<double> fresponseweights;
	getDigiWeights();

	//-------------------------------------------------------------------------------------------------


	//find center of gravity and fill digisOnPlaneTuple
	TNtupleD* digisOnPlaneTuple=new TNtupleD("UVdigipos","","u:v:amp");
	getCogOnPlane(digisOnPlaneTuple);
	//found the cog
	//-------------------------------------------------------------------------------------------------

	//second loop to get shapecov with respect to fCogUV
	getShapeCovFromDigis(planeO,planeU,planeV);


	if(TpcClusterErrorScaler::getInstance()->getScaleMode()>=60)
	{
		if(matrixDebug)
			std::cout<<"getting shapecov from param\n";
		getShapeCovFromParam(UVplane);
		//std::cout<<"cov from parametrization\n";
	}

	if(matrixDebug)
	{
		std::cout<<"make covOnPlane from shapeCov\n";
		fshapeCovPlane.Print();
	}
	TMatrixDSym covOnPlaneSym=TMatrixDSym(2,fshapeCovPlane.GetMatrixArray());

	//got the shape cov

	//-------------------------------------------------------------------------------------------------

	bool doFit=true;
	bool redoCovOnPlane=false;
	int dimToFit=-1;
	Double_t meanOnAxis=0;
	double minSigma[2]={0};
	double minShapeSigma[2]={0};



	//get eigenvalues and vectors on plane
	if(matrixDebug)
		std::cout<<"get eigenvectors from covOnPlaneSym\n";
	TMatrixDSymEigen eigenCovOnPlane(covOnPlaneSym);
	TVectorD eigenValuesOnPlane=eigenCovOnPlane.GetEigenValues();
	TMatrixD eigenVectOnPlane=eigenCovOnPlane.GetEigenVectors();

	//if ndigis==1 construct covOnPlane from geometry
	if( fpadids.size() == 1)
	{
		doFit=false;
		ffitGood=kTRUE;

	}
	else
	{
		int badAxCount=0;

		//loop over axes to check if geom err has to be applied
		fminSigma[0]=0;

		//calculate cluster size
		TVector3 size=calcClusterSize(digisOnPlaneTuple,TVector3(eigenVectOnPlane[0][0],eigenVectOnPlane[1][0],0),TVector3(eigenVectOnPlane[0][1],eigenVectOnPlane[1][1],0));
		for (int i=0 ;i<2; i++)
		{
			Double_t arr[2];
			arr[0]=eigenVectOnPlane[0][i];
			arr[1]=eigenVectOnPlane[1][i];
			TVectorD vec(2,arr);
			//get cut of covonplane with shape cov eigenvector to determine if to small to fit
			if(matrixDebug)
				std::cout<<"get minsigma from geomCovOnPlane\n";
			minSigma[i]=1./(geomCovOnPlane.Similarity(vec));
			minShapeSigma[i]=1./(geomShapeCovOnPlane.Similarity(vec));
			fminSigma[i+1]=minShapeSigma[i];

			//if (eigenValuesOnPlane[i]<1e-8 or eigenValuesOnPlane[i]!=eigenValuesOnPlane[i] )//or eigenValuesOnPlane[i]<minShapeSigma[i])
			if (eigenValuesOnPlane[i]<1e-8 or eigenValuesOnPlane[i]!=eigenValuesOnPlane[i] or size[i]<0.06)
			{
				dimToFit=1-i;
				badAxCount+=1;
				eigenValuesOnPlane[i]=minSigma[i]*minSigma[i];
			}
		}

		//std::cout<<"done comparing :"<<badAxCount<<"\n";
		if (badAxCount==2) //if both axes are bad, dont fit just take geom error
		{doFit=false;}
		else if (dimToFit!=-1) //do the recalculation for the conOnPlane with new eigenvalue
		{redoCovOnPlane=true;}
	}

	//if bad axes were found, recalculate the covOnPlane with new eigenvalues
	if (redoCovOnPlane and doFit)
	{

		TMatrixD S(2,2);
		for (int i=0;i<2;i++)
			for (int j=0;j<2;j++)
				if (i==j)
					S[i][j]=eigenValuesOnPlane[i];

		if(matrixDebug)
			std::cout<<"recalculating fshapeCcovPlane\n";
		fshapeCovPlane=TMatrixDSym(3,(TMatrixD(eigenVectOnPlane,TMatrixD::kMult,
				TMatrixD(S,TMatrixD::kMult,
						TMatrixD(TMatrixD::kInverted,eigenVectOnPlane)
				)
		)).GetMatrixArray()
		);
		//std::cout<<"calculated covOnPlane\n";
		covOnPlaneSym=TMatrixDSym(2,fshapeCovPlane.GetMatrixArray());
	}


	//end checking and correcting cov
	TMatrixDSym fitCov(2);

	//now we fit the digis on plane
	TVector2 shift;
	if (doFit)
	{


		fitCov=fitDigis2D(digisOnPlaneTuple,covOnPlaneSym,shift);
		//	fitCov=fitDigis2D(digisOnPlaneTuple,covOnPlaneSym,digiErr,shift);
		if (fitCov[0][0]==-10)
			doFit=false;

		//calculate new cluster position
		//first transform muU and muV from plane to tpc coordinates
		TVector3 mu=shift.X()*planeU+shift.Y()*planeV;

		//set cluster position exactly in between the planes and add diversion from track
		fpos=planeO+mu;

	}

	if (!doFit or !ffitGood) //if do not fit digis (ndigis==1) then cov=geomCov
	{
		isGeomErr[0]=isGeomErr[1]=isGeomErr[2]=true;
		if(matrixDebug)
			std::cout<<"get fitCov from geomCov\n";
		fitCov=TMatrixDSym(2,TMatrixD(TMatrixD::kInverted,geomCovOnPlane).GetMatrixArray());
		fpos=fCogUV;
	}
	// end fit digis

	//eigenvalues and eigenvectors of fitCov
	if(matrixDebug)
		std::cout<<"get eigenvalues/vects from fitCov\n";
	TMatrixDSymEigen eigen(fitCov);
	TVectorD fiteigenValues=eigen.GetEigenValues();
	TMatrixD fiteigenVect=eigen.GetEigenVectors();

	if (redoCovOnPlane) //if the covOnPlane had to be recalculated (too small axis or single pad)
	{

		TVector3 ev1(fiteigenVect[0][0],fiteigenVect[1][0],0);
		TVector3 ev2(fiteigenVect[0][1],fiteigenVect[1][1],0);
		TVector3 shapeev(eigenVectOnPlane[0][1-dimToFit],eigenVectOnPlane[1][1-dimToFit],0);

		TVectorD cutVec(2);
		if (ev1.Angle(shapeev)<ev2.Angle(shapeev))//determine which eigenvalue has to be reset to geom by angle
		{
			cutVec[0]=ev1[0];
			cutVec[1]=ev1[1];
			if(matrixDebug)
				std::cout<<"get new fiteigenvalue0 from geomCovOnPlane\n";
			fiteigenValues[0]=1./(geomCovOnPlane.Similarity(cutVec));//minSigma[1-dimToFit];
			isGeomErr[1]=true;
		}
		else
		{
			cutVec[0]=ev2[0];
			cutVec[1]=ev2[1];
			if(matrixDebug)
				std::cout<<"get new fiteigenvalue1 from geomCovOnPlane\n";
			fiteigenValues[1]=1./(geomCovOnPlane.Similarity(cutVec));//minSigma[1-dimToFit];
			isGeomErr[2]=true;
		}
		TMatrixD S(2,2);
		for (int i=0;i<2;i++)
			for (int j=0;j<2;j++)
				if (i==j)
					S[i][j]=fiteigenValues[i];

		//recalculate fitcov with new eigenvalues. keep eigenvectors.
		if(matrixDebug)
			std::cout<<"recalculate fitCov with new fiteigenvalues";
		fitCov=TMatrixDSym(2,TMatrixD(TMatrixD(fiteigenVect,TMatrixD::kMult,
				TMatrixD(S,TMatrixD::kMult,
						TMatrixD(TMatrixD::kInverted,fiteigenVect)
				)
		)
		).GetMatrixArray()
		);
	}


	isGeomErr[0]=true;
	feigenValOnPlane.SetXYZ(0,eigenValuesOnPlane[0],eigenValuesOnPlane[1]);
	//now we calculate the covariance from the fit errors
	//the cov from fit gives us 2 eigenvectors, the third one is given by the reclustering planes

	TVector3 eigenVects[3];

	eigenVects[1]=fiteigenVect[0][0]*planeU+fiteigenVect[1][0]*planeV;
	eigenVects[2]=fiteigenVect[0][1]*planeU+fiteigenVect[1][1]*planeV;
	eigenVects[0]=eigenVects[1].Cross(eigenVects[2]);

	if (eigenVects[0]*fmidPlane.getNormal()<0)
	{
		eigenVects[0]*=-1;
	}



	//all eigenvectors need mag=1
	eigenVects[0].SetMag(1.);
	eigenVects[1].SetMag(1.);
	eigenVects[2].SetMag(1.);

	//collect eigenValues
	TVector3 eigenVal;


	double scale0=1.;
	double scale1=1.;


	if (TpcClusterErrorScaler::getInstance()->IsInit())
	{
		if(TpcClusterErrorScaler::getInstance()->getScaleMode()<60)
		{
			scale0=TpcClusterErrorScaler::getInstance()->scaleFunc(fpos.Z(),eigenVects[0].Theta()*TMath::RadToDeg(),0,true);
			scale1=TpcClusterErrorScaler::getInstance()->scaleFunc(fpos.Z(),eigenVects[0].Theta()*TMath::RadToDeg(),1,true);
		}
	}

	if (isGeomErr[1])
		scale0=1.;

	if (isGeomErr[2])
		scale1=1.;

	double geomscale[2];
	geomscale[0]=1;
	geomscale[1]=1;
	TVector3 tmp;

	double nextPad=dx;
	double nextPadX=dx;
	double nextPadY=dy/2*(1+TMath::Sin(30*TMath::DegToRad()) );

	eigenVal.SetX( fdir.Mag2()/12. );//* scale0*scale0 );
	eigenVal.SetY( fiteigenValues[0] * scale0*scale0 * geomscale[0]*geomscale[0] );
	eigenVal.SetZ( fiteigenValues[1] * scale1*scale1 * geomscale[1]*geomscale[1] );

	//std::cout<<"scale0:"<<scale0<<" scale1:"<<scale1<<"\n";

	ferrA.SetXYZ(sqrt(eigenVal[0]),sqrt(eigenVal[1]),sqrt(eigenVal[2]));

	//construct eigenvector and eigenvalue matrix
	TMatrixD S(3,3);
	for (int row=0;row<3;row++)
	{
		feigenVal[row]=eigenVal[row];
		for (int col=0;col<3;col++)
		{
			feigenVect[row][col]=eigenVects[col][row];
			S[row][col]=0;
			if(row==col)
			{
				S[row][col]=eigenVal[row];
			}
		}
	}

	if(matrixDebug)
		std::cout<<"creating final cov\n";
	fcov=TMatrixDSym(3,
			TMatrixD(feigenVect,TMatrixD::kMult,
					TMatrixD(S,TMatrixD::kMult,
							TMatrixD(TMatrixD::kInverted,feigenVect)))
							.GetMatrixArray());
	//std::cout<<"created final cov\n";
	/*
	TMatrixDSymEigen eigen2(fcov);
	TVectorD eigenValues2=eigen2.GetEigenValues();
	TMatrixD eigenVect2=eigen2.GetEigenVectors();

	TVector3 eigenvect2[3];
	for(int ivec=0;ivec<3;ivec++)
	{
		eigenvect2[ivec].SetXYZ(eigenVect2[0][ivec],eigenVect2[1][ivec],eigenVect2[2][ivec]);
		//double scal=eigenvect2[ivec]*fmidPlane.getNormal()
		//if()
	}
	std::cout<<"eigenvect0*midplanenorm:"<<eigenvect2[0]*fmidPlane.getNormal()<<"\n";
	//std::cout<<"eigenvect1*midplanenorm:"<<eigenvect2[1]*fmidPlane.getNormal()<<"\n";
	//std::cout<<"eigenvect2*midplanenorm:"<<eigenvect2[2]*fmidPlane.getNormal()<<"\n";
	//std::cout<<"\n";
	 */


	ferr.SetXYZ(sqrt(fcov[0][0]),sqrt(fcov[1][1]),sqrt(fcov[2][2]));

	/*
	std::cout<<"Plane Vectors:\n";
	planeU.Print();
	planeV.Print();
	planeO.Print();
	std::cout<<"\nThe fit eigenvectors:\n";
	fiteigenVect.Print();
	std::cout<<"\nThe fit eigenvalues\n";
	fiteigenValues.Print();
	std::cout<<"\nThe scaled eigenvalues ("<<scale0<<","<<scale1<<")\n";
	eigenVal.Print();
	std::cout<<"\nEigenvalues\n";
	eigenValues2.Print();
	std::cout<<"\neigenVectors\n";
	eigenVect2.Print();
	 */

	fcovOnPlane=fitCov;

	delete digisOnPlaneTuple;
}

void TpcPrelimCluster::calcError(double zJitter)
{
	bool DEBUG=false;
	double dx, dy;
	TpcDigiMapper::getInstance()->padsize(fdigis[0]->padId(),dx,dy);

	TMatrixD geomCov(3,3);
	geomCov[0][1]=geomCov[0][2]=geomCov[1][0]=geomCov[1][2]=geomCov[2][0]=geomCov[2][1]=0;
	geomCov[0][0]=dx*dx/12;
	geomCov[1][1]=dy*dy/12;
	geomCov[2][2]=zJitter*zJitter/12;

	TVector3 XYZAx[3];
	XYZAx[0]=TVector3(1,0,0);
	XYZAx[1]=TVector3(0,1,0);
	XYZAx[2]=TVector3(0,0,1);

	ferr.SetXYZ(0.,0.,0.);

	//some constants which are maybe needed
	unsigned int ndigis=fdigis.size();
	TVector3 df_0(dx*dx/12,dy*dy/12,zJitter*zJitter/12);
	TVector3 df_00(dx,
			dy/2. * ( 1+ TMath::Sin(30*TMath::DegToRad()) ),
			zJitter);

	Double_t df_sum=dx+dy+zJitter;
	double Dl = TpcDigiMapper::getInstance()->getGas()->Dl();
	double Dt = TpcDigiMapper::getInstance()->getGas()->Dt();
	double driftl = fabs(fpos.z());
	double diffSigmaT = Dt * Dt * driftl;
	double diffSigmaL = Dl * Dl * driftl;
	TVector3 VDiff;
	VDiff.SetXYZ(Dl,Dl,Dt);

	TpcDigi* adigi;
	TVector3 thispos;
	TVector3 df, thissig,thissigT;
	std::vector<TMatrixD> acovs;
	TMatrixD tmpcov(3,3);
	double wamp=0;
	double amp=famp;

	fresponseweights.assign(fdigis.size(),1.);
	if(fDoPRF)
	{
		//std::cout<<"prelimcluster: doing prf shit\n";
		fpos.SetXYZ(0,0,0);
		getDigiWeights();
		//reclalculate position taking prf weights into account
		for(unsigned int id=0; id<ndigis; ++id)
		{
			adigi = fdigis[id];
			//std::cout<<"weight:"<<fresponseweights[id]<<"\n";
			double a = (double)adigi->amp()*fdigiShares[adigi]*fresponseweights[id];
			try
			{
				TpcDigiMapper::getInstance()->map(adigi,thispos);
			}
			catch(const std::exception& ex)
			{
				std::cout<<ex.what()<<std::endl;
			}
			fpos+=a*thispos;
			wamp+=a;
		}
		fpos*=1./wamp;
		amp=wamp;
	}

	for(unsigned int id=0; id<ndigis; ++id)
	{
		adigi = fdigis[id];
		double a = (double)adigi->amp()*fdigiShares[adigi]*fresponseweights[id];

		try
		{
			TpcDigiMapper::getInstance()->map(adigi,thispos);
		}
		catch(const std::exception& ex)
		{
			std::cout<<ex.what()<<std::endl;
		}

		if (frecalcDigiPos)
			thispos=recalcDigiPos(thispos,adigi->padId(),a/famp);

		df = thispos-fpos;

		double sigmaX_sq = a*df.X()*df.X();
		double sigmaY_sq = a*df.Y()*df.Y();
		double sigmaZ_sq = a*df.Z()*df.Z();

		if (phiIsSet)
		{
			double alpha=df.Phi()-fphi;
			double longi=TMath::Cos(alpha)*df.Perp();
			double perp =TMath::Sin(alpha)*df.Perp();

			double sigmaL_sq = a*longi*longi;
			double sigmaP_sq = a*perp*perp;
			thissigT.SetXYZ(sigmaL_sq,sigmaP_sq,sigmaZ_sq);
			ferrT += thissigT;
		}

		TMatrixD c(3,1);
		for(int i=0; i<3; ++i)
		{
			c[i][0]=df[i] ;
		}

		TMatrixD c_t(TMatrixD::kTransposed,c);
		TMatrixD acov(c,TMatrixD::kMult,c_t);
		acov*=a;
		tmpcov+=acov;
		acovs.push_back(acov*a);
		thissig.SetXYZ(sigmaX_sq, sigmaY_sq, sigmaZ_sq);
		ferr  += thissig;

	} // end second loop over digis
	fcov=TMatrixDSym(3,tmpcov.GetMatrixArray());
	ferr*=1./(amp);
	fcov*=1./(amp);
	ferrT*=1./(amp);



	//correct for 0 entries (cov0Cor)
	for (int i=0;i<3;i++)
		for (int j=0;j<3;j++)
			if (i==j)
			{
				if (ndigis==1)
				{
					fcov[i][j]=df_0[i];
					ferr[i]=df_0[i];
				}
				else
				{
					if(fcov[i][j]<1e-10)
						fcov[i][j]=df_0[i];
					if (ferr[i]<1e-10)
						ferr[i]=df_0[i];
				}
			}
			else if (ndigis==1)
				fcov[i][j]=0;

	TMatrixDSymEigen eigen(fcov);
	feigenVect=eigen.GetEigenVectors();
	feigenVal=eigen.GetEigenValues();

	//correct for entries less than min (covMinCor)
	bool redoCov=false;
	geomCov.Invert();
	for (int i=0;i<3;i++)
	{
		double minErr=0;

		Double_t arr[3];

		arr[0]=feigenVect[0][i];
		arr[1]=feigenVect[1][i];
		arr[2]=feigenVect[2][i];
		TVectorD vec(3,arr);
		minErr=1./sqrt(geomCov.Similarity(vec));
		minErr/=famp;
		if (feigenVal[i]<minErr or feigenVal[i]!=feigenVal[i] )
		{
			feigenVal[i]=minErr;
			redoCov=true;
		}
	}

	if (redoCov)
	{
		//std::cout<<"cov:";
		//fcov.Print();
		TMatrixD S(3,3);
		for(int i=0;i<3;i++)
		{
			for(int j=0;j<3;j++)
			{
				if (i==j)
				{
					S[i][j]=feigenVal[i];
				}
				else
					S[i][j]=0.;
			}
		}


		fcov=TMatrixDSym(3,TMatrixD(feigenVect,TMatrixD::kMult,
				TMatrixD(S,TMatrixD::kMult,
						TMatrixD(TMatrixD::kInverted,feigenVect)
				)
		).GetMatrixArray());
		ferr.SetXYZ(fabs(fcov[0][0]),fabs(fcov[1][1]),fabs(fcov[2][2]));
	}

	ferr*=1./(amp);
	fcov*=1./(amp);
	ferrT*=1./(amp);

	if (fG==-1) //standard error
	{
		ferr.SetX(sqrt(ferr.X())*fC);
		ferr.SetY(sqrt(ferr.Y())*fC);
		ferr.SetZ(sqrt(ferr.Z())*fC);

		for (int i=0;i<3;i++)
			for (int j=0;j<3;j++)
				if (i==j)
				{
					fcov[i][j]=ferr[i]*ferr[i];
					feigenVal[i]=ferr[i]*ferr[i];
					feigenVect[i][j]=1;
				}
				else
				{
					fcov[i][j]=0;
					feigenVect[i][j]=0;
				}

		TMatrixD plane(3,2);
		plane[0][0]=fmidPlane.getU().X(); plane[0][1]=fmidPlane.getV().X();
		plane[1][0]=fmidPlane.getU().Y(); plane[1][1]=fmidPlane.getV().Y();
		plane[2][0]=fmidPlane.getU().Z(); plane[2][1]=fmidPlane.getV().Z();

		fcovOnPlane.ResizeTo(fcov);
		fcovOnPlane=TMatrixDSym(3,fcov.GetMatrixArray());
		fcovOnPlane.SimilarityT(plane);

		TMatrixDSymEigen eigenOnPlane(fcovOnPlane);
		TVectorD eval=eigenOnPlane.GetEigenValues();
		TMatrixD evec=eigenOnPlane.GetEigenVectors();

		TVectorD dir(3);
		dir[0]=fmidPlane.getNormal()[0];dir[1]=fmidPlane.getNormal()[1];dir[2]=fmidPlane.getNormal()[2];
		double eval1=fcov.Similarity(dir);

		feigenValOnPlane[0]=eval1;feigenValOnPlane[1]=eval[0];feigenValOnPlane[2]=eval[1];

		TVector3 ev2=evec[0][0]*fmidPlane.getU()+evec[1][0]*fmidPlane.getV();
		TVector3 ev3=evec[0][1]*fmidPlane.getU()+evec[1][1]*fmidPlane.getV();
		feigenVect[0][0]=dir[0]; feigenVect[0][1]=ev2[0]; feigenVect[0][2]=ev3[0];
		feigenVect[1][0]=dir[1]; feigenVect[1][1]=ev2[1]; feigenVect[1][2]=ev3[1];
		feigenVect[2][0]=dir[2]; feigenVect[2][1]=ev2[2]; feigenVect[2][2]=ev3[2];

		//feigenVect.Print();

		fCogUV.SetXYZ((fpos-fmidPlane.getO())*fmidPlane.getU(),(fpos-fmidPlane.getO())*fmidPlane.getV(),0);

		ferrA.SetXYZ(sqrt(feigenValOnPlane[0]),sqrt(feigenValOnPlane[1]),sqrt(feigenValOnPlane[2]));
		ferrT.SetXYZ(sqrt(ferrT.X()),sqrt(ferrT.Y()),sqrt(ferrT.Z()));

		getShapeCovFromDigis(plane);

		return;
	}
	else if(fG==-2) //no error scaling
	{
		ferr.SetX(sqrt(ferr.X()));
		ferr.SetY(sqrt(ferr.Y()));
		ferr.SetZ(sqrt(ferr.Z()));
		TMatrixDSymEigen eigen2(fcov);
		feigenVect=eigen2.GetEigenVectors();
		feigenVal=eigen2.GetEigenValues();

		ferrA.SetXYZ(sqrt(feigenVal[0]),sqrt(feigenVal[1]),sqrt(feigenVal[2]));
		ferrT.SetXYZ(sqrt(ferrT.X()),sqrt(ferrT.Y()),sqrt(ferrT.Z()));
	}
	else //scaled error
	{
		ferr.SetX(sqrt(ferr.X())*fC);
		ferr.SetY(sqrt(ferr.Y())*fC);
		ferr.SetZ(sqrt(ferr.Z())*fC);
		ferrT.SetXYZ(sqrt(ferrT.X()),sqrt(ferrT.Y()),sqrt(ferrT.Z()));

		TMatrixDSymEigen eigen2(fcov);
		feigenVect=eigen2.GetEigenVectors();
		feigenVal=eigen2.GetEigenValues();
		Double_t scale=1;

		TMatrixD S(3,3);
		for(int i=0;i<3;i++)
		{
			for(int j=0;j<3;j++)
			{
				if (i==j)
				{

					TVector3 ev(feigenVect[0][i],feigenVect[1][i],feigenVect[2][i]);
					if (TpcClusterErrorScaler::getInstance()->IsInit())
						scale=TpcClusterErrorScaler::getInstance()->scaleFunc(driftl,i,ev,VDiff);

					ev.SetMag(1);
					ev*=feigenVal[j];
					S[i][j]=feigenVal[j]*scale*scale;
					ferrA[j]=sqrt(feigenVal[j])*scale;
				}
				else
					S[i][j]=0.;

			}
		}


		fcov=TMatrixDSym(3,TMatrixD(feigenVect,TMatrixD::kMult,
				TMatrixD(S,TMatrixD::kMult,
						TMatrixD(TMatrixD::kInverted,feigenVect)
				)
		).GetMatrixArray());
		ferr.SetXYZ(TMath::Sqrt(fcov[0][0]),
				TMath::Sqrt(fcov[1][1]),
				TMath::Sqrt(fcov[2][2]));
	} //end error scaling

	if (fdir.Mag()!=0)
	{
		TVectorD tmp_eigenVal(3);
		TMatrixD tmp_eigenVect(3,3);

		tmp_eigenVal=feigenVal;
		tmp_eigenVect=feigenVect;

		TVector3 ev[3];
		TVector3 zaxis(0,0,1);
		bool used[3]={false};
		int newsort[3]={0};
		for (int row=0;row<3;row++)
		{
			ev[0][row]=feigenVect[row][0];
			ev[1][row]=feigenVect[row][1];
			ev[2][row]=feigenVect[row][2];
		}
		double minangle=10;
		double maxangle=0;
		//find the eigenvect closest to mom
		for(int v=0;v<3;v++)
		{
			if(fabs(fdir.Angle(ev[v])-(TMath::Pi()/2.))>maxangle)
			{
				maxangle=fabs(fdir.Angle(ev[v])-(TMath::Pi()/2.));
				newsort[0]=v;
			}
		}

		used[newsort[0]]=true;
		minangle=10;
		maxangle=0;
		//find the eigenvect
		for(int v=0;v<3;v++)
		{
			if (fabs(zaxis.Angle(ev[v])-(TMath::Pi()/2.)) > maxangle && !used[v])
			{
				minangle=zaxis.Angle(ev[v]);
				maxangle=fabs(zaxis.Angle(ev[v])-(TMath::Pi()/2.));
				newsort[2]=v;
			}
		}

		used[newsort[2]]=true;
		for (int v=0;v<3;v++)
			if (!used[v])
				newsort[1]=v;

		bool changed=false;
		for(int v=0;v<3;v++)
		{
			feigenVal[v]=tmp_eigenVal[newsort[v]];
			feigenVect[v][0]=ev[newsort[0]][v];
			feigenVect[v][1]=ev[newsort[1]][v];
			feigenVect[v][2]=ev[newsort[2]][v];
			if (v!=newsort[v])
				changed=true;

		}
	}

}

TpcPrelimCluster* TpcPrelimCluster::getSharedCluster(TpcDigi* d, unsigned int i)
{
	std::set<TpcPrelimCluster*>::iterator it = fSharedClusters[d].begin();
	for (unsigned int j=0; j<i; ++j)
	{
		++it;
	}
	return *it;
}

TMatrixDSym TpcPrelimCluster::fitDigis2D(TNtupleD* digisOnPlaneTuple, TMatrixDSym covOnPlaneSym, TVector2 digiErr, TVector2& shift)
{

	Double_t u_tmp;
	digisOnPlaneTuple->SetBranchAddress("u",&u_tmp);
	Double_t v_tmp;
	digisOnPlaneTuple->SetBranchAddress("v",&v_tmp);
	Double_t a_tmp;
	digisOnPlaneTuple->SetBranchAddress("amp",&a_tmp);

	//create TGraph2D
	TGraph2DErrors* graph=new TGraph2DErrors();
	for (int idi=0;idi<digisOnPlaneTuple->GetEntries();idi++)
	{
		digisOnPlaneTuple->GetEntry(idi);
		graph->SetPoint(idi,u_tmp,v_tmp,a_tmp);
		graph->SetPointError(idi,digiErr.X(),digiErr.Y(),0.05*a_tmp);
	}

	//prepare fit function
	Double_t umin;
	Double_t umax;
	Double_t vmin;
	Double_t vmax;
	Double_t maxamp;

	//check if closing the range improves things

	umin=digisOnPlaneTuple->GetMinimum("u");
	umax=digisOnPlaneTuple->GetMaximum("u");
	vmin=digisOnPlaneTuple->GetMinimum("v");
	vmax=digisOnPlaneTuple->GetMaximum("v");
	maxamp=digisOnPlaneTuple->GetMaximum("amp");

	double umins = umin * (umin<0?1.5:0.5);
	double umaxs = umax * (umax>0?1.5:0.5);
	double vmins = vmin * (vmin<0?1.5:0.5);
	double vmaxs = vmax * (vmax>0?1.5:0.5);

	TF2* fit2dgauss=new TF2("fit2dgauss",gauss2d,-10,10,-10,10,6);
	fit2dgauss->SetParName(0,"sigU");
	fit2dgauss->SetParName(1,"sigV");
	fit2dgauss->SetParName(2,"rho");

	fit2dgauss->SetParameter(0,TMath::Sqrt(covOnPlaneSym[0][0]));
	fit2dgauss->FixParameter(0,TMath::Sqrt(covOnPlaneSym[0][0]));
	//fit2dgauss->SetParLimits(0,0.9*TMath::Sqrt(covOnPlaneSym[0][0]),1.1*TMath::Sqrt(covOnPlaneSym[0][0]));


	fit2dgauss->SetParameter(1,TMath::Sqrt(covOnPlaneSym[1][1]));
	fit2dgauss->FixParameter(1,TMath::Sqrt(covOnPlaneSym[1][1]));
	//fit2dgauss->SetParLimits(1,0.9*TMath::Sqrt(covOnPlaneSym[1][1]),1.1*TMath::Sqrt(covOnPlaneSym[1][1]));


	if (TMath::Sqrt(covOnPlaneSym[0][0]*covOnPlaneSym[1][1]) != 0)
	{
		fit2dgauss->SetParameter(2,covOnPlaneSym[1][0]/TMath::Sqrt(covOnPlaneSym[0][0]*covOnPlaneSym[1][1]));
		fit2dgauss->FixParameter(2,covOnPlaneSym[1][0]/TMath::Sqrt(covOnPlaneSym[0][0]*covOnPlaneSym[1][1]));
		//fit2dgauss->SetParLimits(2,
		//                        0.9*covOnPlaneSym[1][0]/TMath::Sqrt(covOnPlaneSym[0][0]*covOnPlaneSym[1][1]),
		//                       1.1*covOnPlaneSym[1][0]/TMath::Sqrt(covOnPlaneSym[0][0]*covOnPlaneSym[1][1]))
	}
	else
	{
		fit2dgauss->SetParameter(2,0);
		fit2dgauss->SetParLimits(2,-1,1);
	}
	fit2dgauss->SetRange(umins,vmins,umaxs,vmaxs);
	fit2dgauss->SetParName(3,"MeanU");
	fit2dgauss->SetParameter(3,fCogUV.X());
	fit2dgauss->SetParLimits(3,fCogUV.X()-2*TMath::Sqrt(covOnPlaneSym[0][0]),fCogUV.X()+2*TMath::Sqrt(covOnPlaneSym[0][0]));
	fit2dgauss->SetParName(4,"MeanV");
	fit2dgauss->SetParameter(4,fCogUV.Y());
	fit2dgauss->SetParLimits(4,fCogUV.Y()-2*TMath::Sqrt(covOnPlaneSym[1][1]),fCogUV.Y()+2*TMath::Sqrt(covOnPlaneSym[1][1]));
	fit2dgauss->SetParName(5,"Amp");
	fit2dgauss->SetParameter(5,maxamp);
	fit2dgauss->SetParLimits(5,0.5*maxamp,2*maxamp);

	TFitResultPtr result=graph->Fit("fit2dgauss","S E M Q");
	TMatrixDSym fitcov(6);
	fitcov=result.Get()->GetCovarianceMatrix();
	TMatrixDSym meanCov(2);
	meanCov[0][0]=fitcov[3][3];
	meanCov[1][1]=fitcov[4][4];
	meanCov[0][1]=fitcov[3][4];
	meanCov[1][0]=fitcov[4][3];

	shift=TVector2(fit2dgauss->GetParameter(3),fit2dgauss->GetParameter(4));
	ffitGood=kTRUE;
	delete graph;
	delete fit2dgauss;

	return meanCov;
}

TMatrixDSym TpcPrelimCluster::fitDigis2D(TNtupleD* digisOnPlaneTuple, TMatrixDSym covOnPlaneSym, TVector2& shift)
{
	//create roofit variables
	Double_t umin;
	Double_t umax;
	Double_t vmin;
	Double_t vmax;
	Double_t amax;

	//check if closing the range improves things

	umin=digisOnPlaneTuple->GetMinimum("u");
	umax=digisOnPlaneTuple->GetMaximum("u");
	vmin=digisOnPlaneTuple->GetMinimum("v");
	vmax=digisOnPlaneTuple->GetMaximum("v");

	amax=digisOnPlaneTuple->GetMaximum("amp");
	//std::cout<<"fit2d: maxamp="<<amax<<"\n";


	double umins = umin;// * (umin<0?1.5:0.5);
	double umaxs = umax;// * (umax>0?1.5:0.5);
	double vmins = vmin;// * (vmin<0?1.5:0.5);
	double vmaxs = vmax;// * (vmax>0?1.5:0.5);

	double cogumin = fCogUV.X()-sqrt(covOnPlaneSym[0][0]);
	double cogumax = fCogUV.X()+sqrt(covOnPlaneSym[0][0]);
	double cogvmin = fCogUV.Y()-sqrt(covOnPlaneSym[1][1]);
	double cogvmax = fCogUV.Y()+sqrt(covOnPlaneSym[1][1]);


	RooRealVar varU("u","u",umins,umaxs);
	//varU.setError(digiErr.X());
	RooRealVar varV("v","v",vmins,vmaxs);
	//varV.setError(digiErr.Y());
	//RooRealVar varU("u","u",umin,umax);
	//RooRealVar varV("v","v",vmin,vmax);
	RooRealVar varAmp("amp","amp",0,amax+500);
	RooRealVar muU("muU","Mean U of 2D gaussian",fCogUV.X(),cogumin,cogumax);
	RooRealVar muV("muV","Mean V of 2D gaussian",fCogUV.Y(),cogvmin,cogvmax);

	//create roofit dataset
	RooArgSet varset("digicoords");
	varset.add(varU);
	varset.add(varV);
	varset.add(varAmp);
	RooDataSet dataset("dataSet","digisOnPlane",varset,RooFit::Import(*digisOnPlaneTuple),RooFit::WeightVar("amp"));
	//RooDataSet dataset("dataSet","digisOnPlane",varset,RooFit::Import(*digisOnPlaneTuple));


	//create RooFit PDF
	RooArgList vecUV;
	vecUV.add(varU);
	vecUV.add(varV);
	RooArgList vecMu;
	vecMu.add(muU);
	vecMu.add(muV);
	RooMultiVarGaussian mvg("mvg","multivariant gaussian",vecUV,vecMu,covOnPlaneSym);


	//stupid workaround to get tntuple back into memory 
	//TODO Solve this properly

	for (int idi=0;idi<digisOnPlaneTuple->GetEntries();idi++)
	{
		digisOnPlaneTuple->GetEntry(idi);
		/*
		std::cout<<"*********************************************\n";
		std::cout<<"fitting an cluster now\n";
		std::cout<<"U:"<<u_tmp<<" V:"<<v_tmp<<" Amp:"<<a_tmp<<"\n";
		covOnPlaneSym.Print();
		std::cout<<"*********************************************\n";
		 */
	}

	//std::cout<<"fitting\n";
	//covOnPlaneSym.Print();
	RooFitResult* mvgResult=mvg.fitTo(dataset,
			RooFit::Save(),
			//RooFit::SumW2Error(kTRUE),
			RooFit::SumW2Error(kFALSE),
			RooFit::Verbose(kFALSE),
			RooFit::PrintLevel(-1),
			RooFit::PrintEvalErrors(0),
			RooFit::Warnings(kFALSE));
	//std::cout<<"fitting done\n";
	TMatrixDSym fitCov(2);
	fitCov=mvgResult->covarianceMatrix();
	//std::cout<<"fitted cov\n";
	//fitCov.Print();
	//std::cout<<"error on muU: "<<muU.getError()<<" error on muV:"<<muV.getError()<<"\n";
	fminNll=mvgResult->minNll();

	if (mvgResult->status()!=0 and (mvgResult->covQual()==3 or mvgResult->covQual()==2))// || mvgResult->covQual()!=-1)
	{
		std::cout<<"Status is bad!\n";
		std::cout<<"Fit Status:"<<mvgResult->status()<<" covqual:"<<mvgResult->covQual()<<"\n";
		std::cout<<"cov:\n";
		covOnPlaneSym.Print();
		std::cout<<"err mu U:"<<muU.getError()<<" err mu V:"<<muV.getError()<<"\n";
		fitCov[0][0]=-10;
		ffitGood=kFALSE;
	}
	else
	{
		ffitGood=kTRUE;
	}
	shift=TVector2(muU.getVal(),muV.getVal());
	//std::cout<<"error in U:"<<muU.getError()<<" error in V:"<<muV.getError()<<"\n";
	delete mvgResult;
	return fitCov;
}

Double_t TpcPrelimCluster::fitDigis1D(TNtupleD& digisOnPlaneTuple, double sigma, double mean, Double_t& shift)
{  

	//create roofit variables
	Double_t xmin;
	Double_t xmax;

	xmin=digisOnPlaneTuple.GetMinimum("x")-(sigma*0.5);
	xmax=digisOnPlaneTuple.GetMaximum("x")+(sigma*0.5);

	RooRealVar varX("X","X",xmin,xmax);
	RooRealVar varAmp("amp","amp",0,100000);
	RooRealVar muX("muX","Mean X of 1D gaussian",mean,xmin,xmax);
	RooRealVar sigX("sigX","Sigma in X",sigma,0,3*sigma);

	//create roofit dataset
	RooArgSet varset("digicoords");
	varset.add(varX);
	varset.add(varAmp);

	RooDataSet dataset=RooDataSet("dataset","digis on Axis",&digisOnPlaneTuple,varset,"","amp");

	RooGaussian gauss("gauss","simple gaussian",varX,muX,sigX);
	gauss.fitTo(dataset,
			RooFit::Save(),
			RooFit::SumW2Error(kTRUE),
			RooFit::Verbose(kFALSE),
			RooFit::PrintLevel(-1),
			RooFit::PrintEvalErrors(0),
			RooFit::Warnings(kFALSE));

	shift=muX.getVal();
	return muX.getError();
}

TVector3 TpcPrelimCluster::recalcDigiPos(TVector3 digipos, int padid, double ampfrac)
{
	TVector3 dirToPad=digipos-fpos;
	dirToPad.SetZ(0);
	double weight=fpadplane->GetPad(padid)->GetWeight(ampfrac,dirToPad.X(),dirToPad.Y());

	return digipos-dirToPad*weight*fcorrFactor;
}

void TpcPrelimCluster::getDigiWeights()
{
	double change=1;
	double last_change=1;
	if(fresponseweights.size()!=fdigis.size())
	{
		fresponseweights.clear();
		fresponseweights.assign(fdigis.size(),1.);
	}
	//return;


	double sumamp=0;
	double sumamp_weighted=0;
	double thisweight;
	TVector3 startpos(0,0,0);
	TVector3 startpos_weighted(0,0,0);
	TVector3 thispos;
	TVector3 poca;
	TVector3 dirToPad;
	TpcDigi* aDigi;
	//std::cout<<"making weights:\n";
	//get the start position (center of gravity, NOT amp weighted) and sum of amps
	for(int i=0;i<fdigis.size();i++)
	{
		aDigi=fdigis[i];
		sumamp+=aDigi->amp()*fdigiShares[aDigi];
		thispos.SetXYZ(0,0,0);
		try
		{
			TpcDigiMapper::getInstance()->map(aDigi,thispos);
		}
		catch(const std::exception& ex)
		{
			std::cout<<ex.what()<<std::endl;
		}
		startpos+=thispos;
		//weights.push_back(1);
	}
	startpos*=1./fdigis.size();

	//now the main loop looking for the weights.
	int counter=-1;
	while(fabs(change)>0.001 and counter<250)
	{
		counter++;
		last_change=change;
		sumamp_weighted=0;
		startpos_weighted.SetXYZ(0,0,0);
		//get weight for each digi
		for(int i=0;i<fdigis.size();i++)
		{
			aDigi=fdigis[i];
			thispos.SetXYZ(0,0,0);
			try
			{
				TpcDigiMapper::getInstance()->map(aDigi,thispos);
			}
			catch(const std::exception& ex)
			{
				std::cout<<ex.what()<<std::endl;
			}
			poca=calcPOCAtoLine(startpos,fdir,thispos);
			dirToPad=thispos-poca;
			if (dirToPad.Mag()==0)
			{
				//std::cout<<"dirToPad==0\n";
				thisweight=1;
			}
			else
			{
				//std::cout<<"getWeight: share:"<<fdigiShares[aDigi]<<" sumamp:"<<sumamp<<" dirToPad: ("<<dirToPad.X()<<","<<dirToPad.Y()<<")\n";
				thisweight=fpadplane->GetPad(0)->GetWeight((aDigi->amp()*fdigiShares[aDigi])/sumamp,dirToPad.X(),dirToPad.Y());
			}

			if (thisweight==0)
			{
				//std::cout<<"thisweight==0\n";
				thisweight=1;
			}
			//std::cout<<"Amp:"<<aDigi->amp()<<" share:"<<fdigiShares[aDigi]<<" weights:"<<thisweight<<" \n";
			//weights.at(i)=(aDigi->amp()*fdigiShares[aDigi])/thisweight;
			fresponseweights[i]=1./thisweight;
			startpos_weighted+=thispos*fresponseweights[i]*(aDigi->amp()*fdigiShares[aDigi]); //weighted cog
			sumamp_weighted+=fresponseweights[i]*(aDigi->amp()*fdigiShares[aDigi]);
		}
		startpos_weighted*=1./sumamp_weighted;
		TVector3 shift=0.5*(startpos_weighted-startpos); //don't move to far. damping factor of 0.5 can be too much maybe. has to be checked. no damping leads to wild oscillations
		startpos=startpos+shift;
		change=(shift).Mag();
	}
}

TVector3 TpcPrelimCluster::calcPOCAtoLine(TVector3 refpos, TVector3 dir, TVector3 pos)
{
	TVector3 poca(0,0,0);
	poca= refpos + ( dir * ( ( dir*(pos-refpos) )/dir.Mag() ) );
	return poca;
}

TVector3 TpcPrelimCluster::calcClusterSize(TNtupleD* digisOnPlaneTuple, TVector3 ax0, TVector3 ax1)
{

	TVector3 maxDaxis(-100,-100,-100);
	TVector3 minDaxis(100,100,100);

	Double_t u_tmp;
	digisOnPlaneTuple->SetBranchAddress("u",&u_tmp);
	Double_t v_tmp;
	digisOnPlaneTuple->SetBranchAddress("v",&v_tmp);


	for (int idi=0;idi<digisOnPlaneTuple->GetEntries();idi++)
	{
		digisOnPlaneTuple->GetEntry(idi);
		TVector3 dpos(u_tmp,v_tmp,0);
		TVector3 dist=dpos-fCogUV;
		double a0=ax0*dist;
		double a1=ax1*dist;

		if (a0>maxDaxis.X())
			maxDaxis.SetX(a0);
		if (a0<minDaxis.X())
			minDaxis.SetX(a0);
		if (a1>maxDaxis.Y())
			maxDaxis.SetY(a1);
		if (a1<minDaxis.Y())
			minDaxis.SetY(a1);
	}
	TVector3 size=maxDaxis-minDaxis;
	//	size.Print();
	return size;

}

void TpcPrelimCluster::getGemSpreadCorr(double z, double* corrFactors)
{
	//get XY fraction of U and V
	double xy_of_U=fmidPlane.getU().Perp();
	double xy_of_V=fmidPlane.getV().Perp();
	//get XY correction due to gem spread
	corrFactors[0]=TMath::Sqrt(1 + xy_of_U * ( (0.0343738+0.0286046*TMath::Sqrt(z)) -1 ));
	corrFactors[1]=TMath::Sqrt(1 + xy_of_V * ( (0.0343738+0.0286046*TMath::Sqrt(z)) -1 ));
}

void TpcPrelimCluster::getShapeCovFromParam(TMatrixD UVplane)
{
	TMatrixDSym paramCov(3);
	double varX,varY,varZ;
	TpcClusterErrorScaler::getInstance()->paramVariance(varX,varY,varZ,fcogpos.Z(),fmidPlane.getNormal());
	paramCov[0][0]=varX;
	paramCov[1][1]=varY;
	paramCov[2][2]=varZ;
	//std::cout<<"the paramCov\n";
	//paramCov.Print();

	if(matrixDebug)
		std::cout<<"cutting parametrized cov with UVplane\n";
	//paramCov.Invert();
	fshapeCovPlane=paramCov.SimilarityT(UVplane);
	//fshapeCovPlane.Invert();

	//fshapeCovPlane[0][0]=fabs(fshapeCovPlane[0][0]);
	//fshapeCovPlane[0][1]=fabs(fshapeCovPlane[0][1]);
	//fshapeCovPlane[1][0]=fabs(fshapeCovPlane[1][0]);
	//fshapeCovPlane[1][1]=fabs(fshapeCovPlane[1][1]);
	//fshapeCovPlane.Print();
	//std::cout<<"done paramCov on plane\n";
}

void TpcPrelimCluster::getShapeCovFromDigis(TVector3 planeO, TVector3 planeU, TVector3 planeV)
{
	fRMSUV.SetXYZ(0,0,0);
	fARMSUV.SetXYZ(0,0,0);
	TpcDigi* adigi;
	unsigned int ndigis=fdigis.size();
	TVector3 thispos;
	double amp_weighted=0;
	TMatrixD shapeCov(3,3);
	TMatrixD unwShapeCov(3,3);
	TMatrixD shapeCovPlane(2,2);
	TMatrixD unwShapeCovPlane(2,2);
	for(unsigned int id=0; id<ndigis; ++id)
	{
		adigi = fdigis[id];
		double a;
		a  = (double)adigi->amp();
		a *= fdigiShares[adigi];
		a *= fresponseweights[id];
		amp_weighted+=a;

		thispos.SetXYZ(0,0,0);
		try
		{
			TpcDigiMapper::getInstance()->map(adigi,thispos);
		}
		catch(const std::exception& ex)
		{
			std::cout<<ex.what()<<std::endl;
		}

		TMatrixD c3D(3,1);
		c3D[0][0]=thispos.X()-fpos.X();
		c3D[1][0]=thispos.Y()-fpos.Y();
		c3D[2][0]=thispos.Z()-fpos.Z();
		TMatrixD acov3D(c3D , TMatrixD::kMult , TMatrixD(TMatrixD::kTransposed,c3D) );
		shapeCov+=a*acov3D;
		unwShapeCov+=acov3D;

		thispos=thispos-planeO;

		double thisposU=thispos*planeU/planeU.Mag();
		double thisposV=thispos*planeV/planeV.Mag();


		//calculate outer product of digi-cluster distance for cov
		TMatrixD c(2,1);

		c[0][0]=thisposU-fCogUV.X();
		c[1][0]=thisposV-fCogUV.Y();

		TMatrixD acov(c , TMatrixD::kMult , TMatrixD(TMatrixD::kTransposed,c) );
		//fcov is weighted by amp
		shapeCovPlane+=acov*a;
		unwShapeCovPlane+=acov;
	}
	if(matrixDebug)
		std::cout<<"normalizing covariance matrices from shape\n";;

	fshapeCov=TMatrixDSym(3,shapeCov.GetMatrixArray());
	funwShapeCov=TMatrixDSym(3,unwShapeCov.GetMatrixArray());
	fshapeCovPlane=TMatrixDSym(2,shapeCovPlane.GetMatrixArray());
	funwShapeCovPlane=TMatrixDSym(2,unwShapeCovPlane.GetMatrixArray());

	fshapeCov*=1./famp;
	funwShapeCov*=1./ndigis;
	fshapeCovPlane*=1./amp_weighted;
	funwShapeCovPlane*=1./ndigis;
}

void TpcPrelimCluster::getShapeCovFromDigis(TMatrixD UVplane)
{
	fRMSUV.SetXYZ(0,0,0);
	fARMSUV.SetXYZ(0,0,0);
	TpcDigi* adigi;
	unsigned int ndigis=fdigis.size();
	TVector3 thispos;
	double amp_weighted=0;
	TMatrixD shapeCov(3,3);
	TMatrixD unwShapeCov(3,3);
	for(unsigned int id=0; id<ndigis; ++id)
	{
		adigi = fdigis[id];
		double a;
		a  = (double)adigi->amp();
		a *= fdigiShares[adigi];
		a *= fresponseweights[id];
		amp_weighted+=a;

		thispos.SetXYZ(0,0,0);
		try
		{
			TpcDigiMapper::getInstance()->map(adigi,thispos);
		}
		catch(const std::exception& ex)
		{
			std::cout<<ex.what()<<std::endl;
		}

		TMatrixD c3D(3,1);
		c3D[0][0]=thispos.X()-fpos.X();
		c3D[1][0]=thispos.Y()-fpos.Y();
		c3D[2][0]=thispos.Z()-fpos.Z();
		//std::cout<<"digiPos:: ("<<thispos.X()<<","<<thispos.Y()<<","<<thispos.Z()<<")"<< a<<"\n";
		TMatrixD acov3D(c3D , TMatrixD::kMult , TMatrixD(TMatrixD::kTransposed,c3D) );
		shapeCov+=a*acov3D;
		unwShapeCov+=acov3D;
	}
	if(matrixDebug)
		std::cout<<"normalizing covariance matrices from shape\n";

	//std::cout<<"shapecov after adding\n";
	//shapeCov.Print();

	shapeCov*=1./amp_weighted;
	unwShapeCov*=1./ndigis;

	double wx,wy;
	TpcClusterErrorScaler::getInstance()->getPRFWeight(wx,wy);
	shapeCov[0][0]+=pow(wx/3,2);
	shapeCov[1][1]+=pow(wy/3,2);
	shapeCov[2][2]+=pow(TpcClusterErrorScaler::getInstance()->getZJitter()/6,2);
	//std::cout<<"fwx: "<<wx<<" fwy"<<wy<<" zjitter"<<TpcClusterErrorScaler::getInstance()->getZJitter()<<"\n";


	fshapeCov=TMatrixDSym(3,shapeCov.GetMatrixArray());
	funwShapeCov=TMatrixDSym(3,unwShapeCov.GetMatrixArray());

	fshapeCovPlane=TMatrixDSym(fshapeCov).SimilarityT(UVplane);
	funwShapeCovPlane=TMatrixDSym(funwShapeCov).SimilarityT(UVplane);
}

void TpcPrelimCluster::getCogOnPlane(TNtupleD* digisOnPlaneTuple)
{
	fCogUV.SetXYZ(0,0,0);
	//double amp_weighted=0;
	double sumlength=0;
	fRMS.SetXYZ(0,0,0);
	fARMS.SetXYZ(0,0,0);
	TpcDigi* adigi;
	TVector3 thispos;
	TVector3 planeU=fmidPlane.getU();
	TVector3 planeV=fmidPlane.getV();
	TVector3 planeO=fmidPlane.getO();
	//std::cout<<"plane Vectors\n";
	//planeO.Print();
	//planeU.Print();
	//planeV.Print();
	famp_weighted=0;
	famp2_weighted=0;

	for(unsigned int id=0; id<fdigis.size(); ++id)
	{
		adigi = fdigis[id];
		double a = (double)adigi->amp()*fdigiShares[adigi];
		//std::cout<<"amp before response weight:"<<a<<" weight:"<<fresponseweights[id]<<"\n";
		a *= fresponseweights[id];

		//if(TpcClusterErrorScaler::getInstance()->getScaleMode()>=60)
		//	a*=TpcClusterErrorScaler::getInstance()->paramAmp();

		//mean_ftLength+=adigi->tlength();
		famp_weighted+=a;
		famp2_weighted+=a*a;
		sumlength+=adigi->tlength();

		thispos.SetXYZ(0,0,0);
		try
		{
			TpcDigiMapper::getInstance()->map(adigi,thispos);
		}
		catch(const std::exception& ex)
		{
			std::cout<<ex.what()<<std::endl;
		}

		fRMS.SetXYZ(fRMS.X()+ pow(fpos.X()-thispos.X(),2), fRMS.Y()+ pow((fpos.Y()-thispos.Y()),2), fRMS.Z()+ pow((fpos.Z()-thispos.Z()),2) );
		fARMS.SetXYZ(fARMS.X()+ a*pow(fpos.X()-thispos.X(),2), fARMS.Y()+ a*pow((fpos.Y()-thispos.Y()),2), fARMS.Z()+ a*pow((fpos.Z()-thispos.Z()),2) );

		//be careful here. thispos already transformed here to plane coordinates
		//std::cout<<"original digi pos: ("<<thispos.X()<<","<<thispos.Y()<<","<<thispos.Z()<<")\n";
		thispos=thispos-planeO;

		double thisposU=thispos*planeU/planeU.Mag();
		double thisposV=thispos*planeV/planeV.Mag();

		digisOnPlaneTuple->Fill(thisposU,thisposV,a);
		//std::cout<<"adding digi to tuple: U="<<thisposU<<" V="<<thisposV<<" amp:"<<a<<"\n";

		fCogUV[0] += a*thisposU;
		fCogUV[1] += a*thisposV;
	}
	fCogUV*=1./famp_weighted;
	fRMS*=1./fdigis.size();
	fARMS*=1./famp;
}

TMatrixDSym TpcPrelimCluster::calcHesse()
{
	TMatrixDSym retCov=TMatrixDSym(2,fshapeCovPlane.GetMatrixArray());
	//retCov*=1./(famp_weighted*TpcClusterErrorScaler::getInstance()->paramAmp());

	//hesse error with sumw2 correction
	//retCov*=famp2_weighted/(famp_weighted*famp_weighted);

	//hesse error without sumw2 correction
	retCov*=1./(famp_weighted);
	return retCov;
}