#include "TpcClusterErrorRecalcTask.h"
#include "FairTask.h"
#include "GFTools.h"
#include "TpcSPHit.h"
#include "TpcCluster.h"
#include "TpcClusterErrorScaler.h"
#include "TMatrixDSymEigen.h"
#include "TVectorD.h"
#include "TMatrixD.h"
#include "GFDetPlane.h"
#include "GFTrack.h"
#include "GFTrackCand.h"
#include "GFAbsTrackRep.h"


TpcClusterErrorRecalcTask::TpcClusterErrorRecalcTask()
:fClusterArray(0),
 fSPHitArray(0),
 fTrackArray(0),
 fClusterBranchName("TpcCluster"),
 fSPHitBranchName("TpcSPHit"),
 fTrackBranchName("TrackPostFit")
{}

TpcClusterErrorRecalcTask::~TpcClusterErrorRecalcTask()
{;}

InitStatus TpcClusterErrorRecalcTask::Init()
{
	//Get ROOT Manager ---------------------
	FairRootManager * ioman = FairRootManager::Instance();
	if (ioman==0)
	{
		Error("TpcReClsuterizerTask::Init","Root Manager not instantiated!");
		return kERROR;
	}

	fSPHitArray=(TClonesArray*) ioman->GetObject(fSPHitBranchName);
	if(fSPHitArray==0)
	{
		Fatal("TpcReClusterizerTask::Init","SPHit Array not found");
		return kERROR;
	}

	fClusterArray=(TClonesArray*) ioman->GetObject(fClusterBranchName);
	if (fClusterArray==0)
	{
		Fatal("TpcReClusterizerTask::Init","Cluster Array not found");
		return kERROR;
	}

	fTrackArray=(TClonesArray*) ioman->GetObject(fTrackBranchName);
	if (fTrackArray==0)
	{
		Fatal("TpcReClusterizerTask::Init","Track Array not found");
	}
	return kSUCCESS;
}

void TpcClusterErrorRecalcTask::Exec(Option_t* opt)
{
	std::vector<int> hitIDs;
	FairRootManager* ioman= FairRootManager::Instance();
	GFDetPlane plane;
	TVectorD state(3);
	TMatrixDSym smoothedcov;
	TMatrixD auxinfo(3,3);

	unsigned int nTr = fTrackArray->GetEntriesFast();
	for(unsigned int n=0; n<nTr; n++)
	{
		GFTrack* track = (GFTrack*) fTrackArray->At(n);
		if(track==NULL) continue;

		GFTrackCand cand = track->getCand();
		GFAbsTrackRep* rep = track->getTrackRep(0);
		hitIDs.clear();
		hitIDs = cand.getHitIDs(ioman->GetBranchId(fSPHitBranchName));

		for(int hitID=0; hitID<hitIDs.size(); hitID++)
		{
			TpcSPHit* sphit = (TpcSPHit*)(fSPHitArray->At(hitIDs[hitID]));
			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";

			try
			{
				GFTools::getSmoothedData(track,0,hitID,state,smoothedcov,plane,auxinfo);
			}
			catch(const GFException& ex)
			{
				std::cout << "TpcCosmicsTask(): Could not get smoothed data"  << std::endl;
				std::cout<<ex.what()<<std::endl;
				continue;
			}

			TVector3 o=plane.getNormal();
			TVector3 u=plane.getU();
			TVector3 v=plane.getV();

			TMatrixD planeM(3,2);
			planeM[0][0]=u.X();
			planeM[1][0]=u.Y();
			planeM[2][0]=u.Z();

			planeM[0][1]=v.X();
			planeM[1][1]=v.Y();
			planeM[2][1]=v.Z();

			TMatrix shapeCov(cl->shapeCov());
			TMatrixDSym planeError( TpcClusterErrorScaler::getInstance()->getErrorCovOnPlane(sphit->getClusterID(),planeM) );

			TVectorD oD(3);
			oD[0]=o[0];
			oD[1]=o[1];
			oD[2]=o[2];

			double ew3=fabs(shapeCov.Similarity(oD));

			TMatrixDSymEigen planeErrorEigen(planeError);
			TVectorD planeErrorEigenVal=planeErrorEigen.GetEigenValues();
			TMatrixD planeErrorEigenVec=planeErrorEigen.GetEigenVectors();

			TVector3 covEigenVec1 = planeErrorEigenVec[0][0]*u + planeErrorEigenVec[1][0]*v;
			TVector3 covEigenVec2 = planeErrorEigenVec[0][1]*u + planeErrorEigenVec[1][1]*v;

			TMatrixD M(3,3);
			M[0][0]=o[0]; M[0][1]=covEigenVec1[0]; M[0][2]=covEigenVec2[0];
			M[1][0]=o[1]; M[1][1]=covEigenVec1[1]; M[1][2]=covEigenVec2[1];
			M[2][0]=o[2]; M[2][1]=covEigenVec1[2]; M[2][2]=covEigenVec2[2];

			TMatrixD M_i(M);
			M_i.Invert();

			TMatrixD S(3,3);
			S[0][0]=ew3;
			S[1][1]=planeErrorEigenVal[0];
			S[2][2]=planeErrorEigenVal[1];

			TMatrixD cluster3DCov=M*S*M_i;

			cl->SetCov(cluster3DCov);
			cl->SetSig(TVector3(sqrt(cluster3DCov[0][0]),sqrt(cluster3DCov[1][1]),sqrt(cluster3DCov[2][2])));
			cl->SetSigA(TVector3(sqrt(ew3),sqrt(planeErrorEigenVal[0]),sqrt(planeErrorEigenVal[1])));
			cl->SetMidPlane(plane);
			cl->SetCovOnPlane(planeError);
			cl->SetEigenVect(M);
			cl->SetEigenValOnPlane(TVector3(planeErrorEigenVal[0],planeErrorEigenVal[1],0));

			//std::cout<<"recalcTask:\n";
			//std::cout<<"errorCovOnPLane\n";
			//planeError.Print();
			//std::cout<<"Axis Errors: "<<sqrt(ew3)<<" "<<sqrt(planeErrorEigenVal[0])<<" "<<sqrt(planeErrorEigenVal[1])<<"\n";

		}
	}
}