//-----------------------------------------------------------
// 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 "CollectParticlesTask.h"

#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 "TMatrixDSymEigen.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 "CdcSpacepoint.h"
#include "TpcDigiMapper.h"
#include "TpcPoint.h"
#include "McId.h"
#include "McIdCollection.h"
#include "Particle.h"
#include "TGeoManager.h"
//#include "TpcdEdx.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 "Particle.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);
//}

CollectParticlesTask::CollectParticlesTask()
: fTrackBranchName("FopiTrackPostFit"),
  fOutBranchName("Particles"),
  fTpcHitBranchName("TpcSPHit"),
  fTpcClusterBranchName("TpcCluster"),
  fTpcTrackBranchName("TpcTrackPostFit"),
  fCdcHitBranchName("CdcSpacepoint"),
  fdEdxBranchName("TpcdEdx"),
  fPersistence(kFALSE),
  fTrackArray(),
  fOutArray(),
  fTpcHitArray(),
  fTpcClusterArray(),
  fCdcHitArray(),
  fPar(),
  fskipUnfit(kFALSE),
  fmindEdx(0.),
  fbaddEdx(1000),
  fbadmom(0.08)
{
	;
}

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

InitStatus CollectParticlesTask::Init()
{

	//Get ROOT Manager ------------------
	FairRootManager* ioman= FairRootManager::Instance();
	if(ioman==0)
	{
		Error("Init","RootManager not instantiated!");
		return kERROR;
	}
	// Get input ------------------------
	fTrackArray=(TClonesArray*) ioman->GetObject(fTrackBranchName);
	if(fTrackArray==0)
	{
		std::cout<<"CollectParticlesTask::Init GFTrack-Array: "<<fTrackBranchName<<"\n";
		Error("Init","GFTrack-Array not found!");
		return kERROR;
	}

	fTpcTrackArray=(TClonesArray*) ioman->GetObject(fTpcTrackBranchName);
	if(fTpcTrackArray==0)
	{
		std::cout<<"CollectionParticlesTask::Init TpcTrackArray not found "<<fTpcTrackBranchName<<"\n";
		return kERROR;
	}

	fTpcHitArray=(TClonesArray*) ioman->GetObject(fTpcHitBranchName);
	if(fTpcHitArray==0)
	{
		std::cout<<"CollectParticlesTask::Init TpcHitArray not found ("<<fTpcHitBranchName<<")\n";
		return kERROR;
	}

	fTpcClusterArray=(TClonesArray*) ioman->GetObject(fTpcClusterBranchName);
	if(fTpcClusterArray==0)
	{
		std::cout<<"CollectParticlesTasl::Init TpcClusterArray not found ("<<fTpcClusterBranchName<<")\n";
		return kERROR;
	}

	fCdcHitArray=(TClonesArray*) ioman->GetObject(fCdcHitBranchName);
	if(fCdcHitArray==0)
	{
		std::cout<<"CollectParticlesTask::Init CdcHitArray not found ("<<fCdcHitBranchName<<")\n";
		return kERROR;
	}

	fdEdxArray=(TClonesArray*) ioman->GetObject(fdEdxBranchName);
	if(fdEdxArray==0)
	{
		std::cout<<"CollectParticleTask::Init dEdxArray not found ("<<fdEdxBranchName<<")\n";
		return kERROR;
	}
	fOutArray = new TClonesArray("Particle");
	TString title = fOutBranchName;
	ioman->Register(title,"Particle",fOutArray,fPersistence);

	std::cout<<"INIT OUT ARRAYS\n";


	return kSUCCESS;
}

void CollectParticlesTask::SetParContainers()
{

	std::cout<<"CollectParticlesTask::SetParContainers\n";
	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 CollectParticlesTask::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 CollectParticlesTask::Exec(Option_t* opt)
{
	if(fVerbose) std::cout<<"CollectParticles: start execution\n";
	FairRootManager* ioman= FairRootManager::Instance();


	Int_t ntracks=fTrackArray->GetEntriesFast();

	if(fVerbose) std::cout<<"CollectParticles: start loop over "<<ntracks<<" tracks\n";

	for(unsigned int itr=0;itr<ntracks;++itr)
	{
		GFTrack* trk=(GFTrack*)fTrackArray->At(itr);
		GFTrackCand cand = trk->getCand();
		GFAbsTrackRep* rep=trk->getTrackRep(0);
		TpcdEdx* dedx=(TpcdEdx*)fdEdxArray->At(itr);
		std::vector<int> detIDs = cand.getDetIDs();

		if(fskipUnfit and rep->getStatusFlag()!=0) continue;
		if(dedx->truncMean(0.05,0.25)<fbaddEdx && dedx->getMom().Mag()<fbadmom) continue;

		Particle* mpart=new Particle();
		mpart->SetFitFlag(rep->getStatusFlag());
		mpart->SetdEdx(*dedx);
		if (fdEdxOnly)
		{
			unsigned int nFS = fOutArray->GetEntriesFast();
			new ((*fOutArray)[nFS]) Particle(*mpart);
			delete mpart;
			continue;
		}
		mpart->SetTrackPos(rep->getPos());
		mpart->SetTrackMom(rep->getMom());

		int nhits=cand.getNHits();
		if(fVerbose)std::cout<<"Track: "<<itr<<" Start loop over "<<nhits<<" hits\n";
		for(int ihit=0; ihit<nhits; ihit++)
		{
			int detid;
			int hitid;

			cand.getHit(ihit,detid,hitid);
			TString BrName = ioman->GetBranchName(detid);


			if(fVerbose)std::cout<<"hit: "<<ihit<<" hitID:"<<hitid<<" detid:"<<detid<<" branch:"<<BrName<<"\n";

			TVector3 HitPos;
			TVector3 SigXYZ;

			GFAbsSpacepointHit* SpHit;

			if(BrName==fTpcHitBranchName)
			{
				SpHit=(GFAbsSpacepointHit*) (fTpcHitArray->At(hitid));
				TpcSPHit* hit= (TpcSPHit*)(fTpcHitArray->At(hitid));
				int clid=hit->getClusterID();
				TpcCluster* clust=(TpcCluster*)fTpcClusterArray->At(clid);
				if(fVerbose)
				{
					std::cout<<"clustercov:\n";
					clust->cov().Print();
				}
				mpart->SetHasTpc();
			}
			else if(BrName==fCdcHitBranchName)
			{
				SpHit=(GFAbsSpacepointHit*)	(fCdcHitArray->At(hitid));
				mpart->SetHasCdc();
			}
			else
			{
				if (fVerbose) std::cout<<"Particle Collector: could not find Array for "<<BrName<<"\n";
				continue;
			}

			TVectorD HitPosD(SpHit->getRawHitCoord());
			HitPos.SetXYZ(HitPosD[0],HitPosD[1],HitPosD[2]);

			TMatrixDSym hitCov(SpHit->getRawHitCov());
			//if(BrName==fCdcHitBranchName) hitCov.Print();
			SigXYZ.SetXYZ(hitCov[0][0],hitCov[1][1],hitCov[2][2]);

			//get track point for residual calculation
			Bool_t allok=kTRUE;
			allok=kTRUE;
			GFDetPlane plane;
			TVectorD state;
			TMatrixDSym smcov,cov;
			TMatrixD auxinfo;
			TVector3 TrkPos,mom,TrkPoserr,momerr;

			if (fVerbose) std::cout<<"Particle Collector: trying to get smoothed data\n";

			try
			{
				GFTools::getSmoothedData(trk,0,ihit,state,smcov,plane,auxinfo);
				if(fVerbose)std::cout<<"Particle Collector: got smoothed data\n";
			}
			catch(const GFException& ex)
			{

				std::cout << "Particle Collector: Could not get smoothed data\n";
				std::cout<<ex.what()<<"\n";
				allok=kFALSE;
			}

			if (not allok)
			{
				if (fVerbose) std::cout << "Particle Collector: Try to get biased smoothed data\n";
				try
				{
					GFTools::getBiasedSmoothedData(trk,0,ihit,state,smcov,plane,auxinfo);
					allok=kTRUE;
				}
				catch(const GFException& ex)
				{
					std::cout << "Particle Collector: Could not get biased smoothed data\n";
					std::cout<<ex.what()<<"\n";
					allok=kFALSE;
				}
			}



			if (allok)
			{
				if (fVerbose and allok)std::cout<<"Particle Collector: all ok\n";
				const GFDetPlane plane2(plane);
				const TVectorD state2(state);
				const TMatrixDSym smoothedcov2(smcov);
				const TMatrixD auxinfo2(auxinfo);

				if (fVerbose) std::cout<<"Particle Collector: Got unbiased data and now trying to get PosMomCov\n";
				if(smoothedcov2.GetNcols()>0 && auxinfo2.GetNcols()>0)
				{
					try
					{
						rep->setData(state2,plane2,&smoothedcov2,&auxinfo2);
						rep->getPosMomCov(TrkPos,mom,cov);
						TrkPoserr.SetXYZ(TMath::Sqrt(cov(0,0)),TMath::Sqrt(cov(1,1)),TMath::Sqrt(cov(2,2)));
						momerr.SetXYZ(TMath::Sqrt(cov(3,3)),TMath::Sqrt(cov(4,4)),TMath::Sqrt(cov(5,5)));
					}
					catch(const GFException& ex)
					{
						std::cerr << "Particle Collector: Could not get PosMomCov\n";
						std::cerr<<ex.what()<<"\n";
						continue;
					}
				}
				else
				{
					continue;
				}
				if (fVerbose) std::cout<<"Particle Collector: got unbiased data and PosMomCov\n";
			}
			else
			{
				if (fVerbose) std::cout<<"Particle Collector: Try to get some track point\n";
				try
				{
					plane = SpHit->getDetPlane(rep);
					rep->extrapolate(plane);

				}
				catch(GFException& e)
				{
					std::cerr << "Particle Collector: Could not get DetPlane\n";
					std::cerr << e.what();
					continue;
				}

				try
				{
					rep->getPosMomCov(TrkPos,mom,cov);
					TrkPoserr.SetXYZ(TMath::Sqrt(cov(0,0)),TMath::Sqrt(cov(1,1)),TMath::Sqrt(cov(2,2)));
					momerr.SetXYZ(TMath::Sqrt(cov(3,3)),TMath::Sqrt(cov(4,4)),TMath::Sqrt(cov(5,5)));

				}
				catch(const GFException& ex)
				{
					std::cerr << "Particle Collector: Could not get Position on Plane\n";
					std::cerr<<ex.what()<<"\n";
					continue;
				}

			}

			if(fVerbose)std::cout<<"Particle Collector: appending some data\n";
			mpart->appendPosXYZ(HitPos);
			mpart->appendSigXYZ(SigXYZ);
			mpart->appendDetID(detid);
			mpart->appendDetName(BrName);
			mpart->appendTrackPosXYZ(TrkPos);
			mpart->appendTrackPosXYZErr(TrkPoserr);
			mpart->appendTrackMom(mom);
			mpart->appendTrackMomErr(momerr);

			TVector3 resXYZ(HitPos-TrkPos);
			mpart->appendResXYZ(resXYZ);

			TVector3 planeU, planeV, planeW, planeO;
			planeU=plane.getU();
			planeV=plane.getV();
			planeW=plane.getNormal();
			planeO=plane.getO();
			TVector3 resUV(resXYZ*planeU,resXYZ*planeV,0);
			mpart->appendResUV(resUV);


			TMatrixD planeUm(2,1);
			TMatrixD planeVm(2,1);
			TMatrixD uvplane(3,2);
			for (int i=0;i<3;i++)
			{
				uvplane[i][0]=planeU[i];
				uvplane[i][1]=planeV[i];
			}

			planeUm[0][0]=1;
			planeUm[1][0]=0;
			planeVm[0][0]=0;
			planeVm[1][0]=1;
			if(fVerbose){std::cout<<"cutting cluster cov with detplane for UV errors and residual\n";}
			TMatrixD planeUm_t=TMatrixD(TMatrixD::kTransposed,planeUm);
			TMatrixD planeVm_t=TMatrixD(TMatrixD::kTransposed,planeVm);
			TMatrixD uvplane_t=TMatrixD(TMatrixD::kTransposed,uvplane);
			TMatrixD uvHitCov(2,2);

			if (fVerbose) std::cout<<"ParticleCollector: making uvplane matrix\n";

			uvHitCov=TMatrixD(uvplane_t,TMatrixD::kMult, TMatrixD(hitCov,TMatrixD::kMult,uvplane) );

			TVector3 errUV;
			if(fVerbose) std::cout<<"Particle collector: calculating errorUV\n";
			errUV.SetX(TMath::Sqrt( TMatrixD(planeUm_t,TMatrixD::kMult, TMatrixD(uvHitCov,TMatrixD::kMult,planeUm) )[0][0]) );
			errUV.SetY(TMath::Sqrt( TMatrixD(planeVm_t,TMatrixD::kMult, TMatrixD(uvHitCov,TMatrixD::kMult,planeVm) )[0][0]) );

			mpart->appendSigUV(errUV);
			mpart->appendTrackPlane(plane);

			//calculate axis error
			TMatrixDSymEigen eigen(hitCov);
			TMatrixD eigenVec(3,3);
			eigenVec=eigen.GetEigenVectors();
			TVector3 axis[3];
			//eigenVec.Print();
			for(int i=0;i<3;i++)
			{
				axis[i].SetXYZ(eigenVec[0][i],eigenVec[1][i],eigenVec[2][i]);
			}
			mpart->appendAxis1(axis[0]);
			mpart->appendAxis2(axis[1]);
			mpart->appendAxis3(axis[2]);

			TVector3 resA=TVector3(axis[0]*resXYZ/axis[0].Mag(),axis[1]*resXYZ/axis[1].Mag(),axis[2]*resXYZ/axis[2].Mag());
			TVector3 sigA=TVector3(axis[0].Mag(),axis[1].Mag(),axis[2].Mag());
			//not implemented but has to be filled:
			mpart->appendResA( resA );
			mpart->appendSigA( sigA );
			mpart->appendFullClusterSize(TVector3(0,0,0));
			mpart->appendFullClusterSizeAxis(TVector3(0,0,0));
			mpart->appendFullClusterSizeTrack(TVector3(0,0,0));
		}


		unsigned int nFS = fOutArray->GetEntriesFast();
		new ((*fOutArray)[nFS]) Particle(*mpart);
		delete mpart;
	}

}

void CollectParticlesTask::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<<"CollectParticlesTask::getTrackPosMom():could not extrapolate to point\n";
		std::cerr << e.what();
		return;
	}
	return;
}

ClassImp(CollectParticlesTask)