//-----------------------------------------------------------
// File and Version Information:
// $Id$
//
// Description:
//      Residual calculation for curved tracks
//
//
// Environment:
//      Software developed for the PANDA Detector at FAIR.
//
// Author List:
//      Felix Boehmer           TUM      (original author)
//      Johannes Rauch          TUM
//      Martin Berger
//-----------------------------------------------------------



// This Class' Header ------------------
#include "TpcCosmicsTask.h"

// C++ headers
#include <iostream>
#include <algorithm>
#include <vector>
#include <typeinfo>
#include <exception>

// Collaborating Class Headers --------
#include "FairRootManager.h"
#include "FairRun.h"
#include "FairRuntimeDb.h"
#include "TpcDigiPar.h" 
#include "TMatrixDEigen.h"
#include "TMatrixD.h"
#include "TVectorD.h"

//ROOT
#include "TVector3.h"
#include "TMath.h"
#include "TH1D.h"
#include "TFile.h"
//FOPI ROOT
#include "TpcCluster.h"
#include "TpcSPHit.h"
#include "TpcCosmics.h"
#include "TpcDigiMapper.h"
#include "TpcPoint.h"
#include "McId.h"
#include "McIdCollection.h"

#include "TGeoManager.h"

//Genfick
#include "GFAbsTrackRep.h"
#include "RecoHits/GFAbsRecoHit.h"
#include "GFDetPlane.h"
#include "GFException.h"
#include "GFKalman.h"
#include "GFTools.h"
#include "GFTrack.h"
#include "GFTrackCand.h"

#include "RKTrackRep.h"

#include "PndDetectorList.h"

#define DEBUG 0
#define matrixDebug 0

bool sortpoints(TpcPoint* p1, TpcPoint* p2)
{
	// build temporary McIds and use McId comparison operator
	McId id1(p1->GetEventID(), p1->GetTrackID(), p1->GetSecID());
	McId id2(p2->GetEventID(), p2->GetTrackID(), p2->GetSecID());
	return (id1 < id2);
}

TpcCosmicsTask::TpcCosmicsTask()
: fDigiBranchName("TpcDigi"),
  fClusterBranchName("TpcCluster"),
  fTrackBranchName("TpcTrackPostFit"),
  fOutBranchName("TpcTrackFitStat"),
  fSPHitBranchName("TpcSPHit"),
  fPointBranchName("TpcPoint"),
  fPersistence(kFALSE),
  fSecondarySupp(kFALSE),
  fNumReps(1),
  fVerbose(kFALSE),
  funbiased(kTRUE),
  fClDistCut(3),
  fFile("no"),
  fcutCov(kFALSE),
  fEventcounter(0),
  fBinSize(0,0,0),
  first(true),
  fDigiArray(0),
  fTrackArray(0),
  fClusterArray(0),
  fCosmicsArray(0),
  fSPHitArray(0),
  fResArray(0),
  fPointArray(0),
  fSens(0),
  fPar(),
  faddGTrack(false),
  isMC(false),
  fC(-1),
  fdoDigiMC(false)

{

	;
}

TpcCosmicsTask::~TpcCosmicsTask()
{
	delete fPar;
}

InitStatus TpcCosmicsTask::Init() 
{

	//Get ROOT Manager ------------------
	FairRootManager* ioman= FairRootManager::Instance();
	if(ioman==0)
	{
		Error("TpcCosmicsTask::Init","RootManager not instantiated!");
		return kERROR;
	}
	// Get input ------------------------

	fDigiArray=(TClonesArray*) ioman->GetObject(fDigiBranchName);
	if(fDigiArray==0)
	{
		Error("Init","Digi-Array not found");
		return kERROR;
	}

	fClusterArray=(TClonesArray*) ioman->GetObject(fClusterBranchName);
	if(fClusterArray==0)
	{
		Error("Init","TpcCluster-Array not found!");
		return kERROR;
	}
	fSPHitArray = (TClonesArray*) ioman->GetObject(fSPHitBranchName);
	if(fSPHitArray==0)
	{
		Error("Init","TpcSPHit-Array not found!");
		return kERROR;
	}
	fTrackArray=(TClonesArray*) ioman->GetObject(fTrackBranchName);
	if(fTrackArray==0)
	{
		std::cout<<"GFTrack-Array: "<<fTrackBranchName<<"\n";
		Error("Init","GFTrack-Array not found!");
		return kERROR;
	}

	fPointArray=(TClonesArray*) ioman->GetObject(fPointBranchName);
	isMC=kTRUE;

	if (fPointArray==0)
	{
		std::cout<<"TpcPointsBranchName: "<<fPointBranchName<<"\n";
		Error("Init","Points-Array not found!");
		if (fVerbose)
			std::cout<<"*****************************NO MC ARRAY*************************\n";
		isMC=kFALSE;
	}

	if (!(fFile.EqualTo("no")))
	{
		TFile* hfile = new TFile(fFile.Data());
		TH1D* hsigma=(TH1D*)hfile->Get("SigmaFpnPadIDGlobalRef");
		for (int i=0;i<hsigma->GetNbinsX();i++)
		{
			sigmas.push_back((double)hsigma->GetBinContent(i));
		}
		hfile->Close();
		delete hfile;
	}
	else
		for (int i=0;i<10500;i++)
			sigmas.push_back(-1.);


	std::cout<<"PRE INIT"<<std::endl;

	//GFTrack* tr = (GFTrack*)fTrackArray->At(0);
	//unsigned int nReps = tr->getNumReps();
	for(unsigned int r=0; r<fNumReps; r++) {
		fCosmicsArray = new TClonesArray("TpcCosmics");
		TString title = fOutBranchName;
		title.Append("_");
		char buffer[5];
		sprintf(buffer,"%i",r);
		title.Append(buffer);
		ioman->Register(title,"GenFit",fCosmicsArray,fPersistence);
		fOutArrayMap[r] = fCosmicsArray;
	}

	std::cout<<"INIT OUT ARRAYS"<<std::endl;


	return kSUCCESS;
}

void TpcCosmicsTask::SetParContainers() 
{

	std::cout<<"TpcCosmicsTask::SetParContainers"<<std::endl;
	std::cout.flush();

	// Get run and runtime database
	FairRun* run = FairRun::Instance();
	if ( ! run ) Fatal("SetParContainers", "No analysis run");

	FairRuntimeDb* db = run->GetRuntimeDb();
	if ( ! db ) Fatal("SetParContainers", "No runtime database");

	// Get Tpc digitisation parameter container
	fPar= (TpcDigiPar*) db->getContainer("TpcDigiPar");
	if (!fPar ) Fatal("SetParContainers", "TpcDigiPar not found");
}

double TpcCosmicsTask::Circle(double x_o, double x_n, double y_o, double y_n, 
		double z_o, double z_n, double radius){
	// intercept - inner circle - tracklength calculation
	//Definitons
	double teil=(x_n-x_o);
	double m=(y_n)/(teil);
	double t=(x_o*y_o)/(teil);
	double a=(m*m+1);
	double b=(2*m*t);
	double c=(t*t-radius*radius);
	double disk=(b*b-4*a*c);
	if (disk>0){
		double x1=(-b + TMath::Sqrt(disk)) / (2*a);
		double x2=(-b - TMath::Sqrt(disk)) / (2*a);
		double y1=m*x1+t;
		double y2=m*x2+t;
		double lambda1=(x1-x_o)/(teil);
		double lambda2=(x2-x_o)/(teil);
		double z1=z_o+lambda1*(z_n-z_o);
		double z2=z_o+lambda2*(z_n-z_o);
		double corr=TMath::Sqrt((x1-x2)*(x1-x2)+(y1-y2)*(y1-y2)+(z1-z2)*(z1-z2));
		return corr;
	}
	return 0;
}

void TpcCosmicsTask::Exec(Option_t* opt) {

	if (first)
	{
		double dx,dy;
		TpcDigiMapper::getInstance()->padsize(1,dx,dy);
		fBinSize.SetX(dx);
		fBinSize.SetY(dy);

		//this block is to define the z jitter
		McId dummyID(1,1,0);
		McIdCollection dummyColl;
		dummyColl.AddID(dummyID);

		TVector3 zDiff1,zDiff2;
		TpcDigi zDiffDigi1(1,1,1,dummyColl),zDiffDigi2(1,2,1,dummyColl);
		try
		{
			TpcDigiMapper::getInstance()->map(&zDiffDigi1,zDiff1);
			TpcDigiMapper::getInstance()->map(&zDiffDigi2,zDiff2);
		}
		catch(const std::exception& ex)
		{
			std::cout<<ex.what()<<std::endl;
		}
		double zJitter = zDiff2.z() - zDiff1.z();
		std::cout<<"zJitter:"<<zJitter<<"\n";
		//end of z jitter
		fBinSize.SetZ(zJitter);
		first=false;
	}

	//  std::cout<<"TpcCosmicsTask::Exec()"<<std::endl;

	FairRootManager* ioman= FairRootManager::Instance();

	//clear output

	if (fVerbose)
		std::cout<<"TpcCosmicsTask(): Setting output array; fNumReps: "<<fNumReps<<"\n";

	for(unsigned int r=0; r<fNumReps; r++)
	{
		fCosmicsArray = fOutArrayMap[r];
		if(fCosmicsArray==0) Fatal("TpcCosmicsTask::Exec()","No Cosmics OutputArray");
		fCosmicsArray->Delete();
	}

	//get fit results
	//assert(fTrackArray->GetEntriesFast()<2);

	unsigned int nTr = fTrackArray->GetEntriesFast();

	std::vector<int> candIDs;
	double Dl = TpcDigiMapper::getInstance()->getGas()->Dl();
	double Dt = TpcDigiMapper::getInstance()->getGas()->Dt();
	TVector3 diffusion;
	diffusion.SetXYZ(Dt,Dt,Dl);
	// loop over tracks
	if (fVerbose)
		std::cout<<"TpcCosmicsTask(): Looping over "<<nTr<<" Tracks\n";
	for(unsigned int n=0; n<nTr; n++)
	{
		//    std::cout<<"here1 n:"<<n<<"nTr:"<<nTr<<std::endl;
		GFTrack* track = (GFTrack*) fTrackArray->At(n);
		if(track==NULL) continue;

		if(fVerbose)
			std::cout<<"TpcCosmicsTask::Exec(): Processing track #"<<n<<"\n";


		GFTrackCand cand = track->getCand();
		candIDs.clear();
		candIDs = cand.getHitIDs(ioman->GetBranchId(fSPHitBranchName));

		// loop over trackreps
		if (fVerbose)
			std::cout<<"TpcCosmicsTask(): Looping over "<<fNumReps<<" TrackReps\n";
		for(unsigned int r=0; r<fNumReps; r++) {
			if (fVerbose)
				std::cout<<"TpcCosmicsTask(): Now processing trackrep #:"<<r<<"\n";
			fCosmicsArray = fOutArrayMap[r];

			if (fVerbose)
				std::cout<<"TpcCosmicsTask(): adding new entry to output array\n";
			TpcCosmics* cosmic = new TpcCosmics();//

			GFAbsTrackRep* rep = track->getTrackRep(r);
			//	TVector3 mom = rep->getMom();     //momentum after fit
			TVector3 pos,mom,momerr,poserr,MCpos,MCmom;
			TMatrixDSym cov;
			try
			{
				rep->getPosMomCov(pos,mom,cov);
				//cov.Print();
				poserr=TVector3(TMath::Sqrt(cov(0,0)),TMath::Sqrt(cov(1,1)),TMath::Sqrt(cov(2,2)));
				momerr=TVector3(TMath::Sqrt(cov(3,3)),TMath::Sqrt(cov(4,4)),TMath::Sqrt(cov(5,5)));
			}
			catch(const GFException& ex)
			{
				std::cerr << "TpcCosmicsTask(): Could not get pos mom cov data of initial state"  << std::endl;
				std::cerr<<ex.what()<<std::endl;

				continue;
			}
			int failedHits = track->getFailedHits(r);
			unsigned int NDF = rep->getNDF();
			double Chi2 = rep->getChiSqu();
			int fitflag=rep->getStatusFlag();
			int pdg;
			if (dynamic_cast<RKTrackRep*>(rep)) pdg =  ((RKTrackRep*)rep)->getPDG();
			//else if (dynamic_cast<GeaneTrackRep*>(rep)) pdg =  ((GeaneTrackRep*)rep)->getPDG();
			else
				Fatal("TpcCosmicsTask::Exec()", "Invalid trackrep!");

			if(fVerbose)
				std::cout<<"TpcCosmicsTask(): Adding entry with: mom="<<mom.Mag()<<" theata="<<mom.Theta()*TMath::RadToDeg()<<"\n";

			cosmic->setp(mom.Mag());
			cosmic->setmom(mom);
			cosmic->setMomErr(momerr);
			cosmic->setPos(pos);
			cosmic->setPosErr(poserr);
			cosmic->addFailedHits(failedHits);
			cosmic->setNDF(NDF);
			cosmic->setChi2(Chi2);
			cosmic->setpdg(pdg);
			cosmic->setTheta(mom.Theta()*TMath::RadToDeg());
			cosmic->setPhi(mom.Phi()*TMath::RadToDeg());
			cosmic->setStatFlag(fitflag);

			std::vector<double> resX;
			std::vector<double> resY;
			std::vector<double> resZ;
			std::vector<TVector3> resA;
			std::vector<TVector3> resAMC;
			std::vector<double> resU;
			std::vector<double> resV;
			std::vector<double> resMCU;
			std::vector<double> resMCV;
			std::vector<double> errU;
			std::vector<double> errV;
			std::vector<double> resR;
			std::vector<double> resP;
			std::vector<double> sigX;
			std::vector<double> sigY;
			std::vector<double> sigZ;
			std::vector<TVector3> sigT;
			std::vector<TVector3> sigC;
			std::vector<TVector3> sigA;
			std::vector<Double_t> sigResT;
			std::vector<Double_t> sigResMC;
			std::vector<TVector3> cl_main_ax1;
			std::vector<TVector3> cl_main_ax2;
			std::vector<TVector3> cl_main_ax3;
			std::vector<double> cl_main_ax1_mcerr;
			std::vector<double> cl_main_ax2_mcerr;
			std::vector<double> cl_main_ax3_mcerr;
			std::vector<double> cl_main_ax1_diff;
			std::vector<double> cl_main_ax2_diff;
			std::vector<double> cl_main_ax3_diff;
			std::vector<TVector3> planeU;
			std::vector<TVector3> planeV;
			std::vector<TVector3> planeO;
			std::vector<double> resMcX;
			std::vector<double> resMcY;
			std::vector<double> resMcZ;
			std::vector<double> resMcR;
			std::vector<double> resMcP;
			std::vector<TVector3> mcPosMom;
			std::vector<TVector3> resMCA;
			std::vector<double> mcRefPosX;
			std::vector<double> mcRefPosY;
			std::vector<double> mcRefPosZ;
			std::vector<TVector3> ppsMom;
			std::vector<TVector3> ppsMomErr;

			std::vector<double> chi2X;
			std::vector<double> chi2Y;
			std::vector<double> chi2Z;
			std::vector<double> amps;
			std::vector<double> phis;
			std::vector<unsigned int> clSize;
			std::vector<double> clSizeU;
			std::vector<unsigned int> cl2DSize;
			std::vector<TVector3> fullClSize;
			std::vector<TVector3> fullClSizeTr;
			std::vector<TVector3> fullClSizeAx;
			std::vector<TVector3> pps;
			std::vector<TVector3> ppsErr;
			std::vector<double> posX;
			std::vector<double> posY;
			std::vector<double> posZ;
			std::vector<double> clusdist;
			std::vector<double> rad;
			std::vector<double> mcRefRad;
			std::vector<double> maxDigiAmpl;
			std::vector<bool>   saturated;
			std::vector<double> dsnr;
			std::vector<double> csnr;
			std::vector<std::vector<TVector3> > digisOnPlane;
			std::vector<std::vector<Double_t> > digisOnPlaneAmp;
			std::vector<TVector3> GTrack;
			std::vector<TVector3> isGeomErr;
			std::vector<bool> clusterFitGood;


			TVector3 track_pos;// old_track_pos;
			TVector3 cl_pos, old_cl_pos, ptPos,ptDir, POCA, res3D;
			TVector3 genfZaxis;
			TVector3 genfFirstMC;
			GFAbsRecoHit* hit;
			GFDetPlane plane;
			TVectorD state(3);
			TMatrixDSym smoothedcov;
			TMatrixD auxinfo(3,3);
			McId currentTrackID, currentClusterID;

			//Loop over clusters
			double len=0;
			double clsuma=0;
			double sumz=0;

			if(faddGTrack)
			{
				if(fVerbose)
					std::cout<<"TpcCosmicsTask(): adding genfit track\n";
				TVector3 spos;
				TVector3 smom;
				TVector3 last_pos;
				TVector3 first_pos;
				double step=0;
				try
				{
					spos=rep->getPos();
					first_pos=rep->getPos();
					last_pos=rep->getPos(rep->getLastPlane());
				}
				catch(const GFException& ex)
				{
					std::cout<<"TpcCosmicsTask(): getting first plane\n";
					spos=TVector3(0,0,0);
					first_pos=TVector3(0,0,0);
					last_pos=TVector3(0,0,0);
				}
				double full_dist,dist;
				full_dist=(first_pos-last_pos).Mag();
				dist=(first_pos-spos).Mag();
				while(dist<full_dist and not(spos.Perp()<5 or spos.Perp()>15 or spos.Z()<0 or spos.Z()>80) )
				{
					step+=0.01;

					try
					{
						rep->stepalong(step,spos,smom);
						dist=(first_pos-spos).Mag();
						GTrack.push_back(spos);
					}
					catch(const GFException& ex)
					{
						std::cerr << "TpcCosmicsTask(): Could not step through track"  << std::endl;
						std::cerr<<ex.what()<<std::endl;
						break;
					}
					if(fVerbose)
						std::cout<<"TpcCosmicsTask(): stepping, now at pos ("<<spos.X()<<","<<spos.Y()<<","<<spos.Z()<<")\n";
				}
			}


			std::vector<TpcPoint*> points;
			TVector3 firstMCPoca, firstMCDir;
			TVector3 firstMCpoint;
			if (isMC)
			{
				unsigned int nPoints = fPointArray->GetEntriesFast();
				for(unsigned int i=0; i<nPoints; i++)
				{
					points.push_back((TpcPoint*)((*fPointArray)[i]));
				}
				std::sort(points.begin(), points.end(), sortpoints);


				//try to get the track information at first mc point
				firstMCpoint.SetX( ( (TpcPoint*) (fPointArray)->At(0) )->GetX());
				firstMCpoint.SetY( ( (TpcPoint*) (fPointArray)->At(0) )->GetY());
				firstMCpoint.SetZ( ( (TpcPoint*) (fPointArray)->At(0) )->GetZ());
				//	    ((TpcPoint)(fPointArray->At(0))).Position(firstMCpoint);
				try
				{

					rep->extrapolateToPoint(firstMCpoint,firstMCPoca,firstMCDir);
					cosmic->setFirstMCPocaDir(firstMCPoca,firstMCDir);
				}
				catch(const GFException& ex)
				{
					std::cerr<<"Extrapolation to first MC point failed\n";
					std::cerr<<ex.what()<<"\n";
				}

			}


			//get pos and mom at closest point to z-axis
			TVector3 zAxis1,zAxis2;
			zAxis1=TVector3(0,0,-50);
			zAxis2=TVector3(0,0,50);
			TVector3 zAxisPoca;
			TVector3 zAxisDir;
			TVector3 zAxisWirePoca;

			try
			{
				rep->extrapolateToLine(zAxis1,zAxis2,zAxisPoca,zAxisDir,zAxisWirePoca);
				cosmic->setzAxisPocaDir(zAxisPoca,zAxisDir);
			}
			catch(const GFException& ex)
			{
				std::cerr<<"Extrapolation to line failed\n";
				std::cerr<<ex.what()<<"\n";
			}

			if (fVerbose)
				std::cout<<"Start looping over "<<candIDs.size()<<" Cluster\n";

			for(int k=0; k<candIDs.size(); k++)
			{

				if(fVerbose)
					std::cout<<"Processing Cluster #"<<k<<"\n";
				TpcSPHit* sphit = (TpcSPHit*)(fSPHitArray->At(candIDs[k]));
				if (fVerbose)
					std::cout<<"Getting cluster from sphit\n";
				TpcCluster* cl = (TpcCluster*)(fClusterArray->At(sphit->getClusterID()));
				if (fVerbose)
					std::cout<<"Got cluster from sphit\n";
				if (k>0)
					old_cl_pos=cl_pos;

				cl_pos = cl->pos();
				//std::cout<<"CosmicsTask: cluster pos ("<<cl_pos.X()<<","<<cl_pos.Y()<<","<<cl_pos.Z()<<")\n";
				GFDetPlane midplane=cl->midPlane();;
				sumz+=cl_pos.Z();

				if (fVerbose)
					std::cout<<"Got cluster pos\n";

				clsuma+=cl->amp();

				if (fVerbose)
					std::cout<<"Got cluster amp\n";

				//get 3 main axes of cluster from covarianz matrix * length

				//first get eigenvalues
				TVector3 ax_len(cl->sig(3));
				//std::cout<<"TpcCosmicsTask axis errors: "<<ax_len[0]<<" "<<ax_len[1]<<" "<<ax_len[2]<<"\n";

				std::vector<TVector3> main_ax;
				std::vector<double> main_ax_diff;
				for (int i=0;i<3;++i)
				{
					TVector3 a_ax=cl->axis(i);
					//std::cout<<"TpcCosmicsTask axis "<<i<<": ("<<a_ax[0]<<","<<a_ax[1]<<","<<a_ax[2]<<")\n";
					a_ax*=ax_len[i];
					main_ax.push_back(a_ax);
					main_ax_diff.push_back(a_ax*diffusion/a_ax.Mag());
				}

				if(matrixDebug)
				{std::cout<<"getting eigenvals and eigenvects of cl->cov\n";}
				TMatrixDEigen eigen(cl->cov());
				TMatrixD eigenVec=eigen.GetEigenVectors();
				TMatrixD eigenVec_i(TMatrixD::kInverted,eigenVec);
				TMatrixD eigenVal=eigen.GetEigenValues();
				TMatrixD cl_cov=cl->cov();
				TMatrixD cl_cov_i(TMatrixD::kInverted,cl_cov);
				if(matrixDebug)
				{std::cout<<"got eigenvals and eigenvects of cl->cov\n";}

				std::vector<double> main_ax_mcerr;

				int minId(-1);
				unsigned int nPoints=0;
				if (isMC)
				{
					currentClusterID = cl->mcId().DominantID();
					if (k==0 || currentClusterID != currentTrackID)
						currentTrackID = currentClusterID;

					if (fVerbose)
						std::cout<<"Start calculating MC residual\n";

					double mindist(1.e99);
					double mindistPos(1.e99);

					nPoints = fPointArray->GetEntriesFast();
					//look for the MC Point with minimum distance to cluster
					if (fVerbose)
						std::cout<<"Checking "<<fPointArray->GetEntries()<<" Points\n";
					for (unsigned int iPt=0; iPt<nPoints; iPt++)
					{
						TpcPoint* pt = points[iPt];
						McId idPt(pt->GetEventID(), pt->GetTrackID(), pt->GetSecID());
						if (idPt != currentTrackID)
						{
							continue;
						}

						pt->Position(ptPos);
						double dist = (cl_pos - ptPos).Mag();
						double distPos= (pos - ptPos).Mag();

						if (dist < mindist)
						{
							mindist = dist;
							minId = iPt;
						}
						if (distPos<mindistPos)
						{
							mindistPos=distPos;
							MCpos=ptPos;
							pt->Momentum(MCmom);
						}
					}

					if (fVerbose)
						std::cout<<"found nearest tpcPoint ("<<mindist<<")\n";
					//calc residuum
					if (minId!=-1)
					{
						points[minId]->Position(ptPos);
						points[minId]->Momentum(ptDir);
						Double_t mag=ptDir.Mag();
						ptDir.SetMag(1.);

						// local straight line approx. of the track, calc POCA on line
						POCA = ptPos + ptDir * (ptDir * (cl_pos - ptPos));
						res3D = cl_pos - POCA;
						ptDir.SetMag(mag);
					}
					else
					{
						if(fVerbose)
							std::cout<<"minid was -1 skipped this cluster\n";
						POCA=TVector3(40,40,0);
						res3D = TVector3(-999999,-999999,-99999);
					}

					for (int i=0;i<3;i++)
					{

						main_ax_mcerr.push_back(main_ax[i]*res3D/main_ax[i].Mag());
						main_ax_mcerr[i]/=main_ax[i].Mag();
					}
					TMatrixD C(3,1);
					C[0][0]=res3D.X()/res3D.Mag();
					C[1][0]=res3D.Y()/res3D.Mag();
					C[2][0]=res3D.Z()/res3D.Mag();
					if(matrixDebug)
					{std::cout<<"finding residual scaling factor\n";}
					TMatrixD C_t(TMatrixD::kTransposed,C);
					TMatrixD temp2(3,1);
					if (fcutCov)
						temp2=TMatrixD(cl_cov_i,TMatrixD::kMult,C);
					else
						temp2=TMatrixD(cl_cov,TMatrixD::kMult,C);

					TMatrixD lambda(C_t,TMatrixD::kMult,temp2);

					if(matrixDebug)
					{std::cout<<"got residual scaling factor\n";}
					if (fcutCov)
						sigResMC.push_back(1./TMath::Sqrt(lambda[0][0]));
					else
						sigResMC.push_back(TMath::Sqrt(lambda[0][0]));
				}

				else
				{
					for (int i=0;i<3;i++)
					{
						main_ax_mcerr.push_back(0);
					}
				}


				//calculate residuals and get momentum at track point and other
				if (funbiased)
				{
					Bool_t allok;
					allok=kTRUE;
					if (fVerbose)
						std::cout<<"trying to get unbiased data\n";
					try
					{
						GFTools::getSmoothedData(track,r,k,state,smoothedcov,plane,auxinfo);
					}
					catch(const GFException& ex)
					{

						std::cout << "TpcCosmicsTask(): Could not get smoothed data"  << std::endl;
						std::cout<<ex.what()<<std::endl;
						allok=kFALSE;
						continue;
					}
					if (fVerbose)
						std::cout<<"all ok\n";
					if (allok)
					{
						if (fVerbose)
							std::cout<<"Got unbiased data and now trying to get PosMomCov\n";
						const GFDetPlane plane2(plane);
						const TVectorD state2(state);
						const TMatrixDSym smoothedcov2(smoothedcov);
						const TMatrixD auxinfo2(auxinfo);
						if(smoothedcov2.GetNcols()>0 && auxinfo2.GetNcols()>0)
						{
							try
							{
								rep->setData(state2,plane2,&smoothedcov2,&auxinfo2);
								rep->getPosMomCov(track_pos,mom,cov);
								poserr=TVector3(TMath::Sqrt(cov(0,0)),TMath::Sqrt(cov(1,1)),TMath::Sqrt(cov(2,2)));
								momerr=TVector3(TMath::Sqrt(cov(3,3)),TMath::Sqrt(cov(4,4)),TMath::Sqrt(cov(5,5)));
							}
							catch(const GFException& ex)
							{


								std::cerr << "TpcCosmicsTask():Could not get PosMomCov\n";
								std::cerr<<ex.what()<<std::endl;

								track_pos=TVector3(0,0,0);
								poserr=TVector3(0,0,0);
								mom=TVector3(0,0,0);
								momerr=TVector3(0,0,0);
							}
						}
						else
						{
							track_pos=TVector3(0,0,0);
							poserr=TVector3(0,0,0);
							mom=TVector3(0,0,0);
							momerr=TVector3(0,0,0);
						}
					}
					if (fVerbose)
						std::cout<<"got unbiased data and PosMomCov\n";
				}//end unbiased
				else //biased!!
				{
					try
					{
						if(fVerbose)
							std::cout<<"getting hit:"<<k<<" from "<<track->getNumHits()<<" total hits\n";
						hit = track->getHit(k);
						if (fVerbose)
							std::cout<<"Got track hit\n";
						plane = hit->getDetPlane(rep);
						rep->extrapolate(plane);
					}
					catch(GFException& e)
					{
						std::cerr << "TpcCosmicsTask(): Could not get DetPlane"  << std::endl;
						std::cerr << e.what();
						continue;
					}

					try
					{
						track_pos = rep->getPos(plane);
					}
					catch(const GFException& ex)
					{

						std::cerr << "TpcCosmicsTask(): Could not get Position on Plane"  << std::endl;
						std::cerr<<ex.what()<<std::endl;
						continue;
					}
				}//end biased



				pps.push_back(track_pos);
				//std::cout<<"cosmics task track pos: ("<<track_pos.X()<<","<<track_pos.Y()<<","<<track_pos.Z()<<")\n";
				ppsErr.push_back(poserr);
				ppsMom.push_back(mom);
				ppsMomErr.push_back(momerr);
				//calculate residual
				TVector3 res = cl_pos-track_pos;
				//std::cout<<"cosmicsTask res: ("<<res.X()<<","<<res.Y()<<","<<res.Z()<<")\n";
				TVector3 resMCTrack;
				if (POCA.X()<20 and isMC)
				{
					resMCTrack=POCA-track_pos;
				}
				else
					resMCTrack=TVector3(20,20,80);


				if (k>0)
				{
					clusdist.push_back((old_cl_pos-cl_pos).Mag());

					double mpX=((cl_pos.X()+old_cl_pos.X())/2);
					double mpY=((cl_pos.Y()+old_cl_pos.Y())/2);
					double absolut=(TMath::Sqrt(mpX*mpX+mpY*mpY));
					if (absolut<5.0){
						double cor=Circle(old_cl_pos.X(),cl_pos.X(),old_cl_pos.Y(),
								cl_pos.Y(),old_cl_pos.Z(),cl_pos.Z(),5);
						len+=(clusdist.back()-cor);
					}
					else{
						len+=clusdist.back();
					}
				}
				else
				{
					clusdist.push_back(-1);
				}

				//calculate r and phi residual, UV and Axis
				double reR, reP, reRMC, rePMC;
				double track_pos_phi=-1*track_pos.Phi();

				TVector3 cl_pos_rot=cl_pos;
				cl_pos_rot.RotateZ(track_pos_phi);

				TVector3 track_pos_rot=track_pos;
				track_pos_rot.RotateZ(track_pos_phi);

				reR=cl_pos_rot.X()-track_pos_rot.X();
				reP=cl_pos_rot.Y();
				//no for MC
				if(POCA.X()<20)
				{
					track_pos_phi=-1*POCA.Phi();
					cl_pos_rot=cl_pos;
					cl_pos_rot.RotateZ(track_pos_phi);
					track_pos_rot=POCA;
					track_pos_rot.RotateZ(track_pos_phi);
					reRMC=TMath::Sqrt(cl_pos_rot.X()*cl_pos_rot.X()+cl_pos_rot.Y()*cl_pos_rot.Y())-track_pos_rot.X();
					rePMC=cl_pos_rot.Y();
				}
				else
				{
					reRMC=99999;
					rePMC=99999;
				}

				//calculate residual (track to cluster) in UV coordiantes
				TVector3 resUV;
				resUV.SetX(res*plane.getU());
				resUV.SetY(res*plane.getV());

				//calculates MC to cluster/track residual in UV coordinates
				TVector3 resMCUV;
				TVector3 resMCUVTrack;
				if (isMC)
				{
					resMCUV.SetX(res3D*plane.getU());
					resMCUV.SetY(res3D*plane.getV());
					resMCUVTrack.SetX(resMCTrack*plane.getU());
					resMCUVTrack.SetY(resMCTrack*plane.getV());

				}
				//calculate error U and V
				TVector3 U=plane.getU();
				TVector3 V=plane.getV();
				TVector3 O=plane.getO();
				TVector3 W=plane.getNormal();
				TMatrixD Um(2,1);
				TMatrixD Vm(2,1);
				TMatrixD uvplane=TMatrixD(3,2);
				for (int i=0;i<3;i++)
				{
					uvplane[i][0]=U[i];
					uvplane[i][1]=V[i];
				}

				Um[0][0]=1;
				Um[1][0]=0;
				Vm[0][0]=0;
				Vm[1][0]=1;

				if(matrixDebug) std::cout<<"cutting cluster cov with detplane for UV errors and residual\n";

				TMatrixD Um_t=TMatrixD(TMatrixD::kTransposed,Um);
				TMatrixD Vm_t=TMatrixD(TMatrixD::kTransposed,Vm);
				TMatrixD uvplane_t=TMatrixD(TMatrixD::kTransposed,uvplane);
				TMatrixD cl_uvCov(2,2);
				if (fcutCov){
					cl_uvCov=TMatrixD(uvplane_t,TMatrixD::kMult, TMatrixD(cl_cov_i,TMatrixD::kMult,uvplane) );
				}
				else
					cl_uvCov=TMatrixD(uvplane_t,TMatrixD::kMult, TMatrixD(cl_cov,TMatrixD::kMult,uvplane) );

				TVector3 errUV;
				errUV.SetX(TMath::Sqrt( TMatrixD(Um_t,TMatrixD::kMult, TMatrixD(cl_uvCov,TMatrixD::kMult,Um) )[0][0]) );
				errUV.SetY(TMath::Sqrt( TMatrixD(Vm_t,TMatrixD::kMult, TMatrixD(cl_uvCov,TMatrixD::kMult,Vm) )[0][0]) );
				if(matrixDebug)
				{std::cout<<"done cutting cluster cov with detplane for UV errors and residuals\n";}

				if (fcutCov)
				{
					errUV.SetX(1./errUV.X());
					errUV.SetY(1./errUV.Y());
				}

				//calculate chi2
				TVector3 cl_err= cl->sig();

				TVector3 chi2;
				unsigned int nDim = 4;
				chi2.SetX((res.X()*res.X())/(cl_err.X()*cl_err.X())/nDim);
				chi2.SetY((res.Y()*res.Y())/(cl_err.Y()*cl_err.Y())/nDim);
				chi2.SetZ((res.Z()*res.Z())/(cl_err.Z()*cl_err.Z())/nDim);

				TMatrixD Cf(3,1);
				Cf[0][0]=res.X()/res.Mag();
				Cf[1][0]=res.Y()/res.Mag();
				Cf[2][0]=res.Z()/res.Mag();
				if(matrixDebug)
				{std::cout<<"again residuals scaling factor (not MC)\n";}
				TMatrixD Cf_t(TMatrixD::kTransposed,Cf);
				TMatrixD temp2(3,1);
				if (fcutCov)
					temp2=(cl_cov_i,TMatrixD::kMult,Cf);
				else
					temp2=(cl_cov,TMatrixD::kMult,Cf);

				TMatrixD lambdaf(Cf_t,TMatrixD::kMult,temp2);
				if(matrixDebug)
				{std::cout<<"done residual scaling factor (not MC)\n";}

				if (fcutCov)
					sigResT.push_back(1./TMath::Sqrt(lambdaf[0][0]));
				else
					sigResT.push_back(TMath::Sqrt(lambdaf[0][0]));

				resA.push_back(TVector3(0,0,0));
				resAMC.push_back(TVector3(0,0,0));
				TVector3 resAMCTrack(0,0,0);

				resA.back()[0]=(main_ax[0]*res)/main_ax[0].Mag();
				resA.back()[1]=(main_ax[1]*res)/main_ax[1].Mag();
				resA.back()[2]=(main_ax[2]*res)/main_ax[2].Mag();

				//std::cout<<"cosmicsTask axis res:"<<resA.back()[0]<<" "<<resA.back()[1]<<" "<<resA.back()[2]<<"\n";


				resAMC.back()[0]=(main_ax[0]*res3D)/main_ax[0].Mag();
				resAMC.back()[1]=(main_ax[1]*res3D)/main_ax[1].Mag();
				resAMC.back()[2]=(main_ax[2]*res3D)/main_ax[2].Mag();

				resAMCTrack.SetX( (main_ax[0]*resMCTrack)/main_ax[0].Mag() );
				resAMCTrack.SetY( (main_ax[1]*resMCTrack)/main_ax[1].Mag() );
				resAMCTrack.SetZ( (main_ax[2]*resMCTrack)/main_ax[2].Mag() );

				//caclulate digi dependent variables
				double maxDigiAmp=0;
				bool sat=false;
				TpcDigi* maxDigi;
				TpcDigi* maxSampleDigi;
				double meansigma=0;
				TVector3 minD;
				TVector3 maxD;
				TVector3 minDTrack(100,100,100);
				TVector3 maxDTrack(-100,-100,-100);
				TVector3 minDAxis(100,100,100);
				TVector3 maxDAxis(-100,-100,-100);
				TVector3 posOnPlane(0,0,0);

				TVector3 outerAmp(0,0,0);
				TVector3 maxAbsDist(0,0,0);

				TVector3 innerAmp(0,0,0);
				TVector3 minAbsDist(100,100,100);


				minD.SetXYZ(20,20,80);
				maxD.SetXYZ(-20,-20,-80);
				TVector3 dPos;
				const std::map<unsigned int,double>* digiMap = cl->getDigiMap();
				if (fVerbose)
					std::cout<<"Looping over "<<digiMap->size()<<" digis\n";

				std::vector<TVector3> digiPlaneProj;
				std::vector<Double_t> digiPlaneProjAmp;
				std::vector<TVector3> digiPos;
				std::vector<TVector3> digiMCPos;
				std::vector<double>   digiLen;
				std::vector<TVector3> digiTrackRes;
				std::vector<TVector3> digiTrackMom;
				std::vector<int>      digiID;
				int signAmp=1;
				double maxSampleAmp=0;
				std::vector<TVector3> digiMCRes;
				TMatrixD shapeCov;
				TMatrixD shapeCovOnPlane;
				TMatrixD unwShapeCov;
				TMatrixD unwShapeCovOnPlane;
				//calculate the cluster sizes (along XYZ, along cov axes)
				for(std::map<unsigned int, double>::const_iterator it=digiMap->begin();it!=digiMap->end();it++)
				{
					TpcDigi* aDigi = (TpcDigi*) fDigiArray->At(it->first);
					digiID.push_back(it->first);
					double amp=(double) aDigi->amp()*it->second;

					if (aDigi->getMaxSampleAmp()>maxSampleAmp)
					{
						maxSampleAmp=aDigi->getMaxSampleAmp();
						maxSampleDigi=aDigi;
					}

					if (amp>maxDigiAmp)
					{
						maxDigiAmp=amp;
						maxDigi=aDigi;
					}

					if (aDigi->isSaturated())
						sat=true;
					meansigma+=sigmas[aDigi->padId()];

					Bool_t gotPos=kTRUE;
					try
					{
						TpcDigiMapper::getInstance()->map(aDigi,dPos);
					}
					catch(...)
					{
						std::cerr<<"CosmicsTask: failed to get digi pos\n";
						gotPos=kFALSE;
					}
					if (gotPos)
					{
						digiPos.push_back(dPos);
						digiLen.push_back(aDigi->tlength());

						if(dPos.X()>maxD.X())
							maxD.SetX(dPos.X());
						if(dPos.X()<minD.X())
							minD.SetX(dPos.X());

						if(dPos.Y()>maxD.Y())
							maxD.SetY(dPos.Y());
						if(dPos.Y()<minD.Y())
							minD.SetY(dPos.Y());

						if(dPos.Z()>maxD.Z())
							maxD.SetZ(dPos.Z());
						if(dPos.Z()<minD.Z())
							minD.SetZ(dPos.Z());

						TVector3 cl_digi_dist=dPos-cl->pos();
						double alpha=cl_digi_dist.Phi()-mom.Phi();
						double longi=TMath::Cos(alpha)*cl_digi_dist.Perp();
						double perp=TMath::Sin(alpha)*cl_digi_dist.Perp();
						if (longi>maxDTrack.X())
							maxDTrack.SetX(longi);
						if (longi<minDTrack.X())
							minDTrack.SetX(longi);
						if (perp>maxDTrack.Y())
							maxDTrack.SetY(perp);
						if (perp<minDTrack.Y())
							minDTrack.SetY(perp);

						double a0=cl->axis(0)*cl_digi_dist;
						double a1=cl->axis(1)*cl_digi_dist;
						double a2=cl->axis(2)*cl_digi_dist;

						if(a0>maxDAxis.X())
							maxDAxis.SetX(a0);
						if(a1>maxDAxis.Y())
							maxDAxis.SetY(a1);
						if(a2>maxDAxis.Z())
							maxDAxis.SetZ(a2);

						if(a0<minDAxis.X())
							minDAxis.SetX(a0);
						if(a1<minDAxis.Y())
							minDAxis.SetY(a1);
						if(a2<minDAxis.Z())
							minDAxis.SetZ(a2);


						if(fabs(a0)>maxAbsDist.X())
						{
							maxAbsDist.SetX(fabs(a0));
							outerAmp.SetX(amp);
						}
						if(fabs(a1)>maxAbsDist.Y())
						{
							maxAbsDist.SetY(fabs(a1));
							outerAmp.SetY(amp);
						}
						if(fabs(a2)>maxAbsDist.Z())
						{
							maxAbsDist.SetZ(fabs(a2));
							outerAmp.SetZ(amp);
						}

						if(fabs(a0)<minAbsDist.X())
						{
							minAbsDist.SetX(fabs(a0));
							innerAmp.SetX(amp);
						}
						if(fabs(a1)<minAbsDist.Y())
						{
							minAbsDist.SetY(fabs(a1));
							innerAmp.SetY(amp);
						}
						if(fabs(a2)<minAbsDist.Z())
						{
							minAbsDist.SetZ(fabs(a2));
							innerAmp.SetZ(amp);
						}


						posOnPlane.SetX((dPos-midplane.getO())*midplane.getU()/midplane.getU().Mag());
						posOnPlane.SetY((dPos-midplane.getO())*midplane.getV()/midplane.getV().Mag());

						if ((dPos - midplane.getO())*midplane.getNormal() < 0 )
							signAmp=-1;


						TVector3 diresTrack(-100,-100,-100);
						TVector3 diPoca(-100,-100,-100);
						TVector3 dimom(0,0,0);
						try
						{
							rep->extrapolateToPoint(dPos,diPoca,dimom);
							diresTrack=dPos-diPoca;
						}
						catch(GFException& e)
						{
							//std::cerr<<"TpcCosmicsTask could not extrapolate to digi\n";
						}

						digiTrackRes.push_back(diresTrack);
						digiTrackMom.push_back(dimom);
						/*
		    			posOnPlane.SetX((dPos-O)*U/U.Mag());
		    			posOnPlane.SetY((dPos-O)*V/V.Mag());
						 */
						TVector3 diresMC(-100,-100,-100);
						TVector3 DigiPOCA(0,0,-10);
						if(isMC and fdoDigiMC)
						{
							int iPstart=minId-nPoints/4;
							if (iPstart<0)
								iPstart=0;
							int iPstop=minId+nPoints/4;
							if (iPstop>nPoints)
								iPstop=nPoints;

							double dimindist(1.e99);
							double dimindistPos(1.e99);

							for (unsigned int iPt=iPstart; iPt<iPstop; iPt++)
							{
								TpcPoint* pt = points[iPt];
								McId idPt(pt->GetEventID(), pt->GetTrackID(), pt->GetSecID());
								if (idPt != currentTrackID)
								{
									continue;
								}

								pt->Position(ptPos);
								double didist = (dPos - ptPos).Mag();


								if (didist < dimindist)
								{
									dimindist = didist;
									minId = iPt;
								}
							}

							if (fVerbose)
								std::cout<<"found nearest tpcPoint to digi ("<<dimindist<<")\n";
							//calc residuum
							if (minId!=-1)
							{
								points[minId]->Position(ptPos);
								points[minId]->Momentum(ptDir);
								Double_t mag=ptDir.Mag();
								ptDir.SetMag(1.);

								// local straight line approx. of the track, calc POCA on line
								DigiPOCA = ptPos + ptDir * (ptDir * (dPos - ptPos));
								diresMC = dPos - DigiPOCA;
								ptDir.SetMag(mag);
							}
							else
							{
								if(fVerbose)
									std::cout<<"minid was -1 skipped this cluster\n";
								DigiPOCA=TVector3(40,40,0);
								diresMC = TVector3(-999999,-999999,-99999);
							}
						}//end if(isMC)
						digiMCRes.push_back(diresMC);
						digiMCPos.push_back(DigiPOCA);
					} //end if(got pos)
					digiPlaneProj.push_back(posOnPlane);
					digiPlaneProjAmp.push_back(signAmp*amp);



				}//end loop over digis
				cosmic->appendDigisMCRes(digiMCRes);
				cosmic->appendDigiPos(digiPos);
				cosmic->appendDigiMCPos(digiMCPos);
				cosmic->appendDigiLen(digiLen);
				cosmic->appendDigisTrackRes(digiTrackRes);
				cosmic->appendDigiMom(digiTrackMom);
				cosmic->appendDigiID(digiID);
				cosmic->appendPrfWeights(cl->prfWeights());
				cosmic->appendFieldCorr(cl->fieldCorr());

				TVector3 fullClusterSize;
				TVector3 fullClusterSizeTrack;
				TVector3 fullClusterSizeAxis;
				fullClusterSize=maxD-minD;
				fullClusterSizeTrack=maxDTrack-minDTrack;
				fullClusterSizeAxis=maxDAxis-minDAxis;

				meansigma/=digiMap->size();
				maxDigiAmpl.push_back(maxDigiAmp);
				cosmic->appendMaxSampleAmp(maxSampleAmp);
				int padid=maxDigi->padId();
				int spadid=maxSampleDigi->padId();
				if(fVerbose)
					std::cout<<"calc SNR: "<<maxDigiAmp<<" "<<sigmas[padid]<<" "<<padid<<" "<<maxDigiAmp/sigmas[padid]<<"\n";
				cosmic->appendTpcDSNR(maxDigiAmp/sigmas[padid]);
				cosmic->appendTpcCSNR(cl->amp()/meansigma);
				cosmic->appendTpcSSNR(maxSampleAmp/sigmas[spadid]);

				//dsnr.push_back(maxDigiAmp/sigmas[padid]);
				//csnr.push_back(cl->amp()/meansigma);

				saturated.push_back(sat);
				digisOnPlane.push_back(digiPlaneProj);
				digisOnPlaneAmp.push_back(digiPlaneProjAmp);
				resX.push_back(res.X());
				resY.push_back(res.Y());
				resZ.push_back(res.Z());
				resU.push_back(resUV.X());
				resV.push_back(resUV.Y());
				resMCU.push_back(resMCUV.X());
				resMCV.push_back(resMCUV.Y());
				errU.push_back(errUV.X());
				errV.push_back(errUV.Y());
				resP.push_back(reP);
				resR.push_back(reR);
				sigX.push_back(cl_err.X());
				sigY.push_back(cl_err.Y());
				sigZ.push_back(cl_err.Z());
				sigT.push_back(cl->sig(1));
				sigC.push_back(cl->sig(2));
				sigA.push_back(cl->sig(3));
				cl_main_ax1.push_back(main_ax[0]);
				cl_main_ax2.push_back(main_ax[1]);
				cl_main_ax3.push_back(main_ax[2]);
				cl_main_ax1_mcerr.push_back(main_ax_mcerr[0]);
				cl_main_ax2_mcerr.push_back(main_ax_mcerr[1]);
				cl_main_ax3_mcerr.push_back(main_ax_mcerr[2]);
				cl_main_ax1_diff.push_back(main_ax_diff[0]);
				cl_main_ax2_diff.push_back(main_ax_diff[1]);
				cl_main_ax3_diff.push_back(main_ax_diff[2]);


				planeU.push_back(U);
				planeV.push_back(V);
				planeO.push_back(O);
				cosmic->appendPlaneW(W);

				cosmic->appendMidPlaneU(midplane.getU());
				cosmic->appendMidPlaneV(midplane.getV());
				cosmic->appendMidPlaneW(midplane.getNormal());
				cosmic->appendMidPlaneO(midplane.getO());

				resMcX.push_back(res3D.X());
				resMcY.push_back(res3D.Y());
				resMcZ.push_back(res3D.Z());
				mcPosMom.push_back(ptDir);
				resMcR.push_back(reRMC);
				resMcP.push_back(rePMC);

				mcRefPosX.push_back(POCA.X());
				mcRefPosY.push_back(POCA.Y());
				mcRefPosZ.push_back(POCA.Z());
				mcRefRad.push_back(TMath::Sqrt(POCA.X()*POCA.X()+POCA.Y()*POCA.Y()));

				chi2X.push_back(chi2.X());
				chi2Y.push_back(chi2.Y());
				chi2Z.push_back(chi2.Z());
				amps.push_back(cl->amp());
				phis.push_back(cl->phi());
				clSize.push_back(cl->size());
				clSizeU.push_back(cl->getUnsharedSize());
				cl2DSize.push_back(cl->get2DSize());
				fullClSize.push_back(fullClusterSize);
				fullClSizeTr.push_back(fullClusterSizeTrack);
				fullClSizeAx.push_back(fullClusterSizeAxis);
				posX.push_back(cl_pos.X());
				posY.push_back(cl_pos.Y());
				posZ.push_back(cl_pos.Z());
				rad.push_back(TMath::Sqrt(cl_pos.X()*cl_pos.X()+cl_pos.Y()*cl_pos.Y()));
				isGeomErr.push_back(cl->isGeom());
				clusterFitGood.push_back(cl->fitGood());
				cosmic->appendEigenValOnPlane(cl->eigenValOnPlane());
				cosmic->appendMinSigma(cl->minSigma());
				cosmic->appendResMCTrack(resMCTrack);
				cosmic->appendResMCTrackUV(resMCUVTrack);
				cosmic->appendResMCTrackA(resAMCTrack);
				cosmic->appendCogPos(cl->cogpos());
				cosmic->appendMinNll(cl->minNll());
				cosmic->appendClusterCov(cl->cov());
				cosmic->appendOuterAmp(outerAmp);
				cosmic->appendInnerAmp(innerAmp);
				cosmic->appendCovOnPlane(cl->covOnPlane());
				cosmic->appendShapeCov(cl->shapeCov());
				cosmic->appendUnwShapeCov(cl->unwShapeCov());
				cosmic->appendShapeCovOnPlane(cl->shapeCovPlane());
				cosmic->appendUnwShapeCovOnPlane(cl->unwShapeCovPlane());
				cosmic->appendRMS(TVector3(cl->shapeCov()[0][0],cl->shapeCov()[1][1],cl->shapeCov()[2][2]));
				cosmic->appendUnwRMS(TVector3(cl->unwShapeCov()[0][0],cl->unwShapeCov()[1][1],cl->unwShapeCov()[2][2]));
				cosmic->appendRMS_UV(TVector3(cl->shapeCovPlane()[0][0],cl->shapeCovPlane()[1][1],0));
				cosmic->appendUnwRMS_UV(TVector3(cl->unwShapeCovPlane()[0][0],cl->unwShapeCovPlane()[1][1],0));


				if (cl->shapeCov()[0][0]!=0 or cl->shapeCov()[1][1]!=0 or cl->shapeCov()[2][2]!=0)
				{
					if(matrixDebug)
					{
						std::cout<<"getting eigenvalues of cluster shape cov\n";
						cl->shapeCov().Print();
					}
					TMatrixDEigen eigenshape(cl->shapeCov());
					TMatrixD eigenValShape=eigenshape.GetEigenValues();
					cosmic->appendRMS_A(TVector3(eigenValShape[0][0],eigenValShape[1][1],eigenValShape[2][2]));
					TMatrixDEigen unwEigenShape(cl->unwShapeCov());
					TMatrixD eigenValUnwShape=unwEigenShape.GetEigenValues();
					if(matrixDebug)
					{std::cout<<"got eigenvalues of cluster shape cov\n";}
					cosmic->appendUnwRMS_A(TVector3(eigenValUnwShape[0][0],eigenValUnwShape[1][1],eigenValUnwShape[2][2]));
				}
				else
					cosmic->appendUnwRMS_A(TVector3(0,0,0));

				if(matrixDebug)
				{std::cout<<"getting eigenvalues o shapeCovOnPlane\n";}
				TMatrixDEigen eigenshapeplane(cl->shapeCovPlane());
				TMatrixD eigenValShapePlane=eigenshapeplane.GetEigenValues();
				if(matrixDebug)
				{std::cout<<"got eigenvalues of shapeCovOnPlane, now getting eigenvalues of unwShapeCovPlane\n";}
				cosmic->appendRMS_A_UV(TVector3(eigenValShapePlane[0][0],eigenValShapePlane[1][1],0));
				TMatrixDEigen unwEigenShapePlane(cl->unwShapeCovPlane());
				TMatrixD eigenValUnwShapePlane=unwEigenShapePlane.GetEigenValues();
				cosmic->appendUnwRMS_A_UV(TVector3(eigenValUnwShapePlane[0][0],eigenValUnwShapePlane[1][1],0));
				if(matrixDebug)
				{std::cout<<"got eigenvalues of unwshapeCovPlane\n";}
				double weight=-1;
				try
				{
					weight=track->getBK(-1)->getVector(GFBKKey_dafWeight, k)[0];
				}
				catch(...)
				{
					std::cout<<"TpcCosmicsTask: Could not get weight\n";
				}
				cosmic->appendDAFWeight(weight);

			}//End loop over clusters

			cosmic->fillTpcResX(resX);
			cosmic->fillTpcResY(resY);
			cosmic->fillTpcResZ(resZ);
			cosmic->fillTpcResU(resU);
			cosmic->fillTpcResV(resV);
			cosmic->fillTpcResA(resA);
			cosmic->fillTpcResAMC(resAMC);
			cosmic->fillTpcResMCU(resMCU);
			cosmic->fillTpcResMCV(resMCV);
			cosmic->fillTpcErrU(errU);
			cosmic->fillTpcErrV(errV);
			cosmic->fillTpcResR(resR);
			cosmic->fillTpcResP(resP);
			cosmic->fillTpcSigX(sigX);
			cosmic->fillTpcSigY(sigY);
			cosmic->fillTpcSigZ(sigZ);
			cosmic->fillTpcSigT(sigT);
			cosmic->fillTpcSigC(sigC);
			cosmic->fillTpcSigA(sigA);
			cosmic->fillTpcSigResT(sigResT);
			cosmic->fillTpcSigResMC(sigResMC);
			cosmic->fillTpcAxis1(cl_main_ax1);
			cosmic->fillTpcAxis2(cl_main_ax2);
			cosmic->fillTpcAxis3(cl_main_ax3);
			cosmic->fillTpcAxis1MCErr(cl_main_ax1_mcerr);
			cosmic->fillTpcAxis2MCErr(cl_main_ax2_mcerr);
			cosmic->fillTpcAxis3MCErr(cl_main_ax3_mcerr);
			cosmic->fillTpcAxis1Diff(cl_main_ax1_diff);
			cosmic->fillTpcAxis2Diff(cl_main_ax2_diff);
			cosmic->fillTpcAxis3Diff(cl_main_ax3_diff);
			cosmic->fillPlaneU(planeU);
			cosmic->fillPlaneV(planeV);
			cosmic->fillPlaneO(planeO);
			//cosmic->fillTpcEigenValOnPlane()

			cosmic->fillTpcResMcX(resMcX);
			cosmic->fillTpcResMcY(resMcY);
			cosmic->fillTpcResMcZ(resMcZ);
			cosmic->fillTpcResMcP(resMcP);
			cosmic->fillTpcResMcR(resMcR);
			cosmic->fillTpcMcPosMom(mcPosMom);

			cosmic->fillTpcMcRefPosX(mcRefPosX);
			cosmic->fillTpcMcRefPosY(mcRefPosY);
			cosmic->fillTpcMcRefPosZ(mcRefPosZ);
			cosmic->fillTpcMcRefPosR(mcRefRad);

			cosmic->fillGTrack(GTrack);

			cosmic->fillTpcChi2X(chi2X);
			cosmic->fillTpcChi2Y(chi2Y);
			cosmic->fillTpcChi2Z(chi2Z);
			cosmic->fillTpcClusterSize(clSize);
			cosmic->fillTpcClusterSizeU(clSizeU);
			cosmic->fillTpcFullClusterSize(fullClSize);
			cosmic->fillTpcFullClusterSizeTrack(fullClSizeTr);
			cosmic->fillTpcFullClusterSizeAxis(fullClSizeAx);
			cosmic->fillTpc2DClusterSize(cl2DSize);
			cosmic->fillTpcClusterAmp(amps);
			cosmic->fillTpcClusterPhi(phis);
			cosmic->fillTpcClusterDists(clusdist);
			cosmic->fillTpcProjPoints(pps);
			cosmic->fillTpcProjPointsErr(ppsErr);
			cosmic->fillTpcProjPointsMom(ppsMom);
			cosmic->fillTpcProjPointsMomErr(ppsMomErr);
			cosmic->fillTpcHitPositionsX(posX);
			cosmic->fillTpcHitPositionsY(posY);
			cosmic->fillTpcHitPositionsZ(posZ);
			cosmic->fillTpcHitPositionsR(rad);
			cosmic->fillTpcHitIDs(candIDs);
			cosmic->fillTpcMaxDigiAmp(maxDigiAmpl);
			cosmic->fillTpcDigisOnPlane(digisOnPlane);
			cosmic->fillTpcDigisOnPlaneAmp(digisOnPlaneAmp);
			cosmic->fillTpcSaturationFlag(saturated);
			//cosmic->fillTpcDSNR(dsnr);
			//cosmic->fillTpcCSNR(csnr);

			cosmic->fillTpcIsGeomErr(isGeomErr);
			cosmic->fillTpcClusterFitGood(clusterFitGood);

			cosmic->setLength(len);
			cosmic->setClSumAmp(clsuma);
			cosmic->setEvent(fEventcounter);
			cosmic->setMCpos(MCpos);
			cosmic->setMCmom(MCmom);
			if(candIDs.size()>0)
			{
				cosmic->setClMeanAmp(clsuma/candIDs.size());
				cosmic->setMeanZ(sumz/candIDs.size());
			}
			else
			{
				cosmic->setClMeanAmp(-1);
				cosmic->setMeanZ(-99999);
			}

			// sort vectors to find median
			std::sort(amps.begin(),amps.end());
			std::sort(cl2DSize.begin(),cl2DSize.end());
			std::sort(clSize.begin(),clSize.end());

			//get median and fill to tree
			if (amps.size()>0)
			{
				int medi=(int)(candIDs.size()/2);
				//	  std::cout<<"TpcCosmicsTask: medi: "<<medi<<" candIDs.size()="<<candIDs.size()<<" amps.size()="<<amps.size()<<" 2d sizelen: "<<cl2DSize.size()<<"\n";
				cosmic->setClMediAmp(amps[medi]);
				//	  std::cout<<"here1\n";
				cosmic->setClSizeMed2D(cl2DSize[medi]);
				//	  std::cout<<"here2\n";
				cosmic->setClSizeMed3D(clSize[medi]);
			}
			else
			{
				cosmic->setClMediAmp(-1);
				cosmic->setClSizeMed2D(-1);
				cosmic->setClSizeMed3D(-1);
			}

			unsigned int nFS = fCosmicsArray->GetEntriesFast();
			new ((*fCosmicsArray)[nFS]) TpcCosmics(*cosmic);

			delete cosmic;
		}// end loop over trackreps
	}// end loop over tracks
	fEventcounter+=1;
	if (fVerbose)
		std::cout<<"Cosmics Task done\n";
}

void TpcCosmicsTask::getTrackPosMom(GFAbsTrackRep* _rep, TVector3& _pos, TVector3& _poserr, TVector3& _mom, TVector3& _momerr)
{
	TVector3 poca;
	TVector3 mom;
	try
	{
		_rep->extrapolateToPoint(_pos,poca,mom);
	}
	catch(GFException& e)
	{
		std::cerr<<"TpcCosmicsTask::getTrackPosMom():could not extrapolate to point\n";
		std::cerr << e.what();
		return;
	}

	const GFDetPlane plane(poca,mom);
	const TVectorD state();
	const TMatrixDSym smoothedcov();
	const TMatrixD auxinfo();
}

/*
TVector2 TpcCosmicsTask::ProjVectorOnPlane(TVector3 u,TVector3 v,TVector3 vect)
{
  TVector2 proj=TVector2();
  proj.SetX(
}
 */

ClassImp(TpcCosmicsTask)