// -------------------------------------------------------------------------
// -----                      PndMQStraightLineTrackFinder                    -----
// -----                  Created 22/10/09  by M. Michel               -----
// -------------------------------------------------------------------------

#include "TClonesArray.h"
#include "TArrayD.h"
#include "TGeoManager.h"

#include "FairRootManager.h"
#include "FairRun.h"
#include "FairRuntimeDb.h"
#include "FairMQLogger.h"

#include "PndMQStraightLineTrackFinder.h"

#include "PndSdsDigiStrip.h"
#include "TStopwatch.h"
#include "PndTrkFitter.h"
// #include "PndSdsPixelCluster.h"


// -----   Default constructor   -------------------------------------------
PndMQStraightLineTrackFinder::PndMQStraightLineTrackFinder() :
 fNLayers(4), fEventNr(0)
{
   fdXY = 0.1;
   fHitsPerLayer.resize(fNLayers);
}


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


// -----   Destructor   ----------------------------------------------------
PndMQStraightLineTrackFinder::~PndMQStraightLineTrackFinder()
{
}
// -------------------------------------------------------------------------

// -----   Private method SortHitsByDet   --------------------------------------------
void PndMQStraightLineTrackFinder::SortHitsToLayers()
{
	ClearLayerInfo();
	if (fHits.size() > 3){
		//LOG(INFO) << "SortHitsToLayers: nHits: " << fHits.size() << std::endl;
		for (int i = 0; i < fHits.size(); i++){
		  if (fHits[i].GetSensorID() < fNLayers){
			  fHitsPerLayer[fHits[i].GetSensorID()].push_back(make_pair(i, false));
			  //LOG(INFO) << "SensorID: " << fHits[i].GetSensorID();
		  }
		}
	}
}

int PndMQStraightLineTrackFinder::NLayersFilled()
{
	int result = 0;
	for (auto layer : fHitsPerLayer){
		if (layer.size() > 0)
			result++;
	}
	return result;
}


void PndMQStraightLineTrackFinder::ClearLayerInfo()
{
	for (int layer = 0; layer < fHitsPerLayer.size(); layer++)
		fHitsPerLayer[layer].clear();
}
// -------------------------------------------------------------------------

std::vector< std::vector<int> > PndMQStraightLineTrackFinder::GetStartCombinations(int firstLayer, int secondLayer)
{
	std::vector<std::vector<int> > result;
	for (int firstL = 0; firstL < fHitsPerLayer[firstLayer].size(); firstL++){
		for (int secondL = 0; secondL < fHitsPerLayer[secondLayer].size(); secondL++){
			std::vector<int> singleCombi;
			singleCombi.push_back(fHitsPerLayer[firstLayer][firstL].first);
			singleCombi.push_back(fHitsPerLayer[secondLayer][secondL].first);
			result.push_back(singleCombi);
		}
	}
	return result;
}

StraightLineParams PndMQStraightLineTrackFinder::GetLineParameters(std::vector<int> startCombi)
{
	StraightLineParams result;
	TVector3 first, second;
	bool firstFound = false;
	for (int i = 0; i < startCombi.size(); i++)
	{
		if (startCombi[i] > -1){
			if (firstFound == false){
				PndSdsHit firstHit = fHits[startCombi[i]];
				first = firstHit.GetPosition();
				firstFound = true;
				result.origin = first;
				//LOG(INFO) << "GetLineParameters: first " << firstHit;
			} else {

				PndSdsHit secondHit = fHits[startCombi[i]];
				second = secondHit.GetPosition();
				result.direction = (second - first).Unit();
				//LOG(INFO) << "GetLineParameters: second: " << secondHit;
				break;
			}
			//LOG(INFO) << "LineParams: orig: " << result.origin.X() << "/" << result.origin.Y() << "/" << result.origin.Y()
			//		<< " dir: " << result.direction.X() << "/" << result.direction.Y() << "/" << result.direction.Z();
		}
	}
	return result;
}

TVector3 PndMQStraightLineTrackFinder::PropagateToXYPlane(StraightLineParams line, double z)
{
	double t = 0;
	t = (z - line.origin.Z()) / line.direction.Z();
	TVector3 result;
	result = line.origin + line.direction * t;
	//LOG(INFO) << "Propagate to z= " << z << " : " << result.X() << "/" << result.Y() << "/" << result.Z();
	return result;
}

double PndMQStraightLineTrackFinder::DistanceOfPoints(TVector3 first, TVector3 second)
{
	return (first - second).Mag();
}

PndSimpleTrack PndMQStraightLineTrackFinder::GenerateTrackParams(std::vector<int>& hitsInTrack)
{
	PndTrackCand trackCand;
	double timeStamp = 0;

	for (int i = 0; i < hitsInTrack.size(); i++){
		trackCand.AddHit(FairLink(-1,fEventNr,1,hitsInTrack[i]), i);
		timeStamp += fHits[hitsInTrack[i]].GetTimeStamp();
	}
	StraightLineParams params = FitTrack(hitsInTrack);
	FairTrackPar par(params.origin.X(), params.origin.Y(), params.origin.Z(),
						params.direction.X(), params.direction.Y(), params.direction.Z(), 1);
	timeStamp /= hitsInTrack.size();
	trackCand.SetTimeStamp(timeStamp);
	PndSimpleTrack result(par, trackCand, params.chi2);
//	.SetTrackCand(trackCand);
	return result;
}

StraightLineParams PndMQStraightLineTrackFinder::FitTrack(std::vector<int> hitsInTrack)
{
	PndTrkFitter fitterXZ, fitterYZ;
	for (auto hitId : hitsInTrack){
		fitterXZ.SetPointToFit(fHits[hitId].GetZ(), fHits[hitId].GetX(), fHits[hitId].GetDx());
		fitterYZ.SetPointToFit(fHits[hitId].GetZ(), fHits[hitId].GetY(), fHits[hitId].GetDy());
	}
	double mXZ, pXZ, mYZ, pYZ;
	double chi2XZ = fitterXZ.StraightLineFitWithChi2(mXZ, pXZ);
	double chi2YZ = fitterYZ.StraightLineFitWithChi2(mYZ, pYZ);

	double chi2 = TMath::Sqrt(TMath::Power(chi2XZ,2) + TMath::Power(chi2YZ,2));

	StraightLineParams result;
	result.origin.SetXYZ(pXZ, pYZ, 0);
	result.direction.SetXYZ(pXZ, pYZ, 1);
	result.direction = result.direction.Unit();
	result.chi2 = chi2;
	return result;
}

PndSimpleTrack PndMQStraightLineTrackFinder::FindTrack(std::vector<int>& startCombi, int lastStartPoint){
	StraightLineParams params = GetLineParameters(startCombi);
	std::pair<double, int> closestPoint(-1, -1);
	PndSimpleTrack result;
	for (int layer = lastStartPoint + 1; layer < fNLayers; layer++){
		if (fHitsPerLayer[layer].size() > 0){
			double z = fHits[fHitsPerLayer[layer][0].first].GetZ();
			TVector3 prop = PropagateToXYPlane(params, z);
			for (int point = 0; point < fHitsPerLayer[layer].size(); point++){
				double distance = DistanceOfPoints(prop, fHits[fHitsPerLayer[layer][point].first].GetPosition());
				//LOG(INFO) << " distance: " << distance << " between prop: " << prop.X() << "/" << prop.Y() << "/" << prop.Z() << " and hit: " << fHits[fHitsPerLayer[layer][point].first];
				if ((closestPoint.second < 0) || (closestPoint.first > distance)){
					closestPoint.first = distance;
					closestPoint.second = point;
				}
			}
			if (closestPoint.first < fdXY){
				//LOG(INFO) << "PointAdded!";
				startCombi.push_back(closestPoint.second);
			}
		}
	}
	if (startCombi.size() > 3){
		//LOG(INFO) << "FOUND TRACK!";
		result = GenerateTrackParams(startCombi);
	}
	return result;
}

std::vector<PndSimpleTrack> PndMQStraightLineTrackFinder::FindTracks(std::vector<PndSdsHit> hits, int eventNr)
{
	fEventNr = eventNr;
	fHits = hits;

	std::vector<PndSimpleTrack> result;

	SortHitsToLayers();

	if (NLayersFilled() < 4) {
		return result;
	}
	//LOG(INFO) << "LayersFilled: " << NLayersFilled() << std::endl;
	std::vector<std::vector<int> > startCombi = GetStartCombinations(0,1);
	for (int i = 0; i < startCombi.size(); i++){
		PndSimpleTrack track = FindTrack(startCombi[i], 1);
//		LOG(INFO) << " Track with " << track.GetTrackCand().GetNHits();
		if (track.GetTrackCand().GetNHits() > 0){
//			LOG(INFO) << "TrackFound: " << track.GetNLinks();
			result.push_back(track);

		}
	}
	return result;
}

// -----   Private method FindHitsII  --------------------------------------------
/*void PndMQStraightLineTrackFinder::FindHitsII(std::vector<PndTrackCand> &tofill, std::vector< std::vector< std::pair<Int_t,bool> > > &hitsd, Int_t nStripHits)
{

  std::vector<TVector3> trackStart, trackVec;//pseudo tracks
  std::vector<TVector3> trackStartd, trackVecd;//save errors 
  std::vector< Int_t > trackID1, trackID3; //for TrackCand

//iterate first discs-hits with all seconds, save pseudo-tracks
  TVector3 start, tmp, vec, dstart, dvec; //temp-vars

  if(hitsd.size()<3) return;
  for (Int_t i=0; i<hitsd.at(0).size(); i++)
    {
    if(hitsd.at(0).at(i).second) continue; //if already used
    PndSdsHit *hit1=(PndSdsHit*)fHitArray->At(hitsd.at(0).at(i).first);
    start.SetXYZ(hit1->GetX(), hit1->GetY(), hit1->GetZ());
    dstart.SetXYZ(hit1->GetDx(), hit1->GetDy(), hit1->GetDz());
    for (Int_t k=0; k<hitsd.at(2).size(); k++)
    {
      if(hitsd.at(2).at(k).second) continue; //if already used
      PndSdsHit *hit2=(PndSdsHit*)fHitArray->At(hitsd.at(2).at(k).first);
      tmp.SetXYZ(hit2->GetX(), hit2->GetY(), hit2->GetZ());
      vec = tmp - start;   //calc direction vector for FINDING
      dvec.SetXYZ(hit2->GetDx(), hit2->GetDy(), hit2->GetDz());
      if(vec.Theta()>0.03 && vec.Theta()<0.05 && vec.Phi()>-0.25 && vec.Phi()<0.25){ //ignore vectors with theta outside 2-9 mrad
      // if(0<1){
      //	if(vec.Theta()<0.01){ //ignore vectors with theta outside 10 mrad
        trackStart.push_back(start);
        trackStartd.push_back(dstart);
        trackVec.push_back(vec);        //save vector from start to second
        trackVecd.push_back(dvec); //save error of second point for FIT, NOT error of direction vector
        //trackID1.push_back(hitsd.at(0).at(i).first);  //save Hit-Id's for TrackCand
        //trackID2.push_back(hitsd.at(1).at(k).first);
        trackID1.push_back(i);  //save Hit-Id's for TrackCand
        trackID3.push_back(k);
      }
    }//end of disc 2 (or 3 if none in disc 2) hits
  }//end of disc 1 hits

  if(fVerbose>1) cout << "Pseudos L1L3: "<< trackStart.size() <<endl;

  std::vector<Int_t> ids;

//check if other discs have hits in track, add points for fitting ---------------
  for (Int_t i=0; i<trackStart.size(); i++) //pseudo-loop
  {
    ids.clear();
    Int_t pntcnt=2;
    
    start = trackStart.at(i);
    dstart = trackStartd.at(i);
    vec = trackVec.at(i);
    dvec = trackVecd.at(i);
    ids.push_back(hitsd.at(0).at(trackID1.at(i)).first);
    ids.push_back(hitsd.at(2).at(trackID3.at(i)).first);
    std::vector< std::pair<Int_t,Int_t> > otherIDs;

    for (Int_t idet=3; idet < 4; idet++){ //i know this is not a loop, but in case we someday will have more planes
      Double_t distClosest = 2*idet*dXY; //just bigger as possible
      Bool_t firstp = true;
      for (Int_t ihit=0; ihit<hitsd.at(idet).size(); ihit++)
      {
        PndSdsHit *hit=(PndSdsHit*)fHitArray->At(hitsd.at(idet).at(ihit).first);
        Double_t scale = (hit->GetZ()-start.z())/vec.z();
        tmp = start + scale*vec; //extend search-vector to hit-plane
        Double_t distTmp = sqrt((tmp.x()-hit->GetX())*(tmp.x()-hit->GetX()) + (tmp.y()-hit->GetY())*(tmp.y()-hit->GetY()));
        if( distTmp<(idet*dXY) ){ //if in diameter
          if(firstp){
            pntcnt++;
            ids.push_back(hitsd.at(idet).at(ihit).first);
            otherIDs.push_back( make_pair (idet,ihit) );
            distClosest=distTmp;
            firstp=false;
          }else{
            if(distTmp<distClosest){ //change if nearer
              ids.pop_back();
              otherIDs.pop_back();
              ids.push_back(hitsd.at(idet).at(ihit).first); 
              otherIDs.push_back( make_pair (idet,ihit) );
            }            
          }
        }//endif in diameter
      }//end of disc n hits
    }//end of discs

    if(fVerbose>2) cout << "  Track L1L3: "<< i << "#Planes: " << ids.size() <<endl;

    //create track für fitting
    if(ids.size()>2){ //third hit found <=> !track found!
      PndTrackCand *myTCand = new PndTrackCand();

      for (Int_t id=0; id<ids.size(); id++){
        PndSdsHit* myHit = (PndSdsHit*)(fHitArray->At(ids.at(id)));
        PndSdsClusterStrip* myCluster;
        PndSdsDigiStrip* astripdigi;
        if ( !fStripClusterArray || !fStripDigiArray){ //for ideal/fast Hit-Reco
          myTCand->AddHit(0,ids.at(id),myHit->GetPosition().Mag()); 
        }else{
          myCluster =  (PndSdsClusterStrip*)(fStripClusterArray->At(myHit->GetClusterIndex()));
          astripdigi = (PndSdsDigiStrip*)fStripDigiArray->At(myCluster->GetDigiIndex(0));
          myTCand->AddHit(astripdigi->GetDetID(),ids.at(id),myHit->GetPosition().Mag());  
        }
      }

      //mark used hits---------
      hitsd.at(0).at(trackID1.at(i)).second=true;
      hitsd.at(2).at(trackID3.at(i)).second=true;
      for(Int_t id=0; id<otherIDs.size(); id++){
        hitsd.at(otherIDs.at(id).first).at(otherIDs.at(id).second).second=true;
      }

      // ///Add seed information to track------------
      // PndSdsHit* myHit0 = (PndSdsHit*)(fStripHitArray->At(ids.at(0)));
      // PndSdsHit* myHit1 = (PndSdsHit*)(fStripHitArray->At(ids.at(1)));
      // TVector3 hit0 = myHit0->GetPosition(); TVector3 hit1 = myHit1->GetPosition();
      // TVector3 posSeed(hit0.X(),hit0.Y(),hit0.Z());
      // vec*=1./vec.Mag();
      // 	//shift trk out of plane [needed for correct treatment in Kalman Fillter and GEANE]
      // 	double sh_z = -0.035; //350 mkm
      // 	double sh_x = vec.X()*sh_z;
      // 	double sh_y = vec.Y()*sh_y;
      // 	TVector3 sh_point(sh_x,sh_y,sh_z);
      // 	posSeed +=sh_point;
      // myTCand->setTrackSeed(posSeed,vec,-1);
      // ///-------------------------------------

      tofill.push_back(*(myTCand)); //save Track Candidate
      //new((*fTrackCandArray)[trackCnt]) PndTrackCand(*(myTCand)); 
      delete myTCand;
    }//Track Cand build
  }//end of pseudo-tracks

}
*/
// -------------------------------------------------------------------------

// -----   Private method FindHitsI  --------------------------------------------
/*void PndMQStraightLineTrackFinder::FindHitsI(std::vector<PndTrackCand> &tofill, std::vector< std::vector< std::pair<Int_t,bool> > > &hitsd, Int_t nStripHits)
{

  std::vector<TVector3> trackStart, trackVec;//pseudo tracks
  std::vector<TVector3> trackStartd, trackVecd;//save errors 
  std::vector< Int_t > trackID1, trackID2; //for TrackCand

//iterate first discs-hits with all seconds, save pseudo-tracks
  TVector3 start, tmp, vec, dstart, dvec; //temp-vars

  if(hitsd.size()<2) return;
  for (Int_t i=0; i<hitsd.at(0).size(); i++)
    {
    PndSdsHit *hit1=(PndSdsHit*)fHitArray->At(hitsd.at(0).at(i).first);
    start.SetXYZ(hit1->GetX(), hit1->GetY(), hit1->GetZ());
    dstart.SetXYZ(hit1->GetDx(), hit1->GetDy(), hit1->GetDz());
    for (Int_t k=0; k<hitsd.at(1).size(); k++)
    {
      PndSdsHit *hit2=(PndSdsHit*)fHitArray->At(hitsd.at(1).at(k).first);
      tmp.SetXYZ(hit2->GetX(), hit2->GetY(), hit2->GetZ());
      vec = tmp - start;   //calc direction vector for FINDING
      dvec.SetXYZ(hit2->GetDx(), hit2->GetDy(), hit2->GetDz());
     // if(vec.Theta()>0.03 && vec.Theta()<0.05 && vec.Phi()>-0.25 && vec.Phi()<0.25){ //ignore vectors with theta outside 2-9 mrad
      //      if(vec.Theta()<0.01){ //ignore vectors with theta outside 10 mrad
        trackStart.push_back(start);
        trackStartd.push_back(dstart);
        trackVec.push_back(vec);        //save vector from start to second
        trackVecd.push_back(dvec); //save error of second point for FIT, NOT error of direction vector
        //trackID1.push_back(hitsd.at(0).at(i).first);  //save Hit-Id's for TrackCand
        //trackID2.push_back(hitsd.at(1).at(k).first);
        trackID1.push_back(i);  //save Hit-Id's for TrackCand
        trackID2.push_back(k);
   //   }
    }//end of disc 2 (or 3 if none in disc 2) hits
  }//end of disc 1 hits

  if(fVerbose>1) cout << "Pseudos L1L2: "<< trackStart.size() <<endl;

  std::vector<Int_t> ids;

//check if other discs have hits in track, add points for fitting ---------------
  for (Int_t i=0; i<trackStart.size(); i++) //pseudo-loop
  {
    ids.clear();
    Int_t pntcnt=2;
    
    start = trackStart.at(i);
    dstart = trackStartd.at(i);
    vec = trackVec.at(i);
    dvec = trackVecd.at(i);
    ids.push_back(hitsd.at(0).at(trackID1.at(i)).first);
    ids.push_back(hitsd.at(1).at(trackID2.at(i)).first);
    std::vector< std::pair<Int_t,Int_t> > otherIDs;

    for (Int_t idet=2; idet < 4; idet++){
      Double_t distClosest = 2*idet*dXY; //just bigger as possible
      Bool_t firstp = true;
      for (Int_t ihit=0; ihit<hitsd.at(idet).size(); ihit++)
      {
        PndSdsHit *hit=(PndSdsHit*)fHitArray->At(hitsd.at(idet).at(ihit).first);
        Double_t scale = (hit->GetZ()-start.z())/vec.z();
        tmp = start + scale*vec; //extend search-vector to hit-plane
        Double_t distTmp = sqrt((tmp.x()-hit->GetX())*(tmp.x()-hit->GetX()) + (tmp.y()-hit->GetY())*(tmp.y()-hit->GetY()));
        if( distTmp<(idet*dXY) ){ //if in diameter
          if(firstp){
            pntcnt++;
            ids.push_back(hitsd.at(idet).at(ihit).first);
            otherIDs.push_back( make_pair (idet,ihit) );
            distClosest=distTmp;
            firstp=false;
          }else{
            if(distTmp<distClosest){ //change if nearer
              ids.pop_back();
              otherIDs.pop_back();
              ids.push_back(hitsd.at(idet).at(ihit).first); 
              otherIDs.push_back( make_pair (idet,ihit) );
            }            
          }
        }//endif in diameter
      }//end of disc n hits
    }//end of discs

    if(fVerbose>2) cout << "  Track L1L2: "<< i << "#Planes: " << ids.size() <<endl;

    //create track für fitting
    if(ids.size()>2){ //third hit found <=> !track found!
    //   if(ids.size()>3){ //4 hits are needed because 6 parameters are used in trk-fit!
    // // //third hit found <=> !track found!
    // // //check direction for reduction of ghost tracks
    // if(ids.size()>2 && 
    //    fabs(vec.Phi())<0.26 && vec.Theta()>3e-2 && vec.Theta()<5e-2){ 
      if(fVerbose>2) cout << "  Track: "<< i << "#Planes: " << ids.size() <<endl;
      PndTrackCand *myTCand = new PndTrackCand();
      
      for (Int_t id=0; id<ids.size(); id++){
        PndSdsHit* myHit = (PndSdsHit*)(fHitArray->At(ids.at(id)));
        PndSdsClusterStrip* myCluster;
        PndSdsDigiStrip* astripdigi;

        myTCand->AddHit(FairRootManager::Instance()->GetBranchId(fHitBranchStrip),ids.at(id),myHit->GetPosition().Mag());

      }

      //mark used hits---------
      hitsd.at(0).at(trackID1.at(i)).second=true;
      hitsd.at(1).at(trackID2.at(i)).second=true;
      for(Int_t id=0; id<otherIDs.size(); id++){
        hitsd.at(otherIDs.at(id).first).at(otherIDs.at(id).second).second=true;
      }

      ///Add seed information to track------------
      // Double_t Z0 = 1099.;
      // //  Double_t Z0 = 0;
      // PndSdsHit* myHit0 = (PndSdsHit*)(fStripHitArray->At(ids.at(0)));
      // PndSdsHit* myHit1 = (PndSdsHit*)(fStripHitArray->At(ids.at(1)));
      // TVector3 hit0 = myHit0->GetPosition(); TVector3 hit1 = myHit1->GetPosition();
      // double p1seed = (hit0.X()-hit1.X())/(hit0.Z()-hit1.Z());
      // double p0seed = hit0.X() - p1seed*(hit0.Z()-Z0);
      // //      double p0seed = 0.5*(hit0.X()+hit1.X()-p1seed*(hit0.Z()+hit1.Z()-2*1099.)); //TO DO: don't use const
      // double p3seed = (hit0.Y()-hit1.Y())/(hit0.Z()-hit1.Z());
      // double p2seed = hit0.Y() - p3seed*(hit0.Z()-Z0);
      // //double p2seed = 0.5*(hit0.Y()+hit1.Y()-p1seed*(hit0.Z()+hit1.Z()-2*1099.)); //TO DO: don't use const
      // TVector3 posSeed(p0seed,p2seed,Z0);
      // TVector3 dirSeed(p1seed,p3seed,1.);
      //   cout<<"posSeed:"<<endl;
      //   posSeed.Print();
      //   cout<<"dirSeed:"<<endl;
      //   dirSeed.Print();

      // //  Double_t Z0 = 1099.;
      // //     TVector3 posSeed(start.X(),start.Y(),Z0);
      // TVector3 posSeed(start.X(),start.Y(),start.Z());
      // // cout<<"Trk cand: pos"<<endl;
      // // posSeed.Print();
      // vec*=1./vec.Mag();
      // //shift trk out of plane [needed for correct treatment in Kalman Fillter and GEANE]
      // double sh_z = -0.035; //350 mkm
      // double sh_x = vec.X()*sh_z;
      // double sh_y = vec.Y()*sh_y;
      // TVector3 sh_point(sh_x,sh_y,sh_z);
      // posSeed +=sh_point;

      // // cout<<"Trk cand: vec"<<endl;
      // // vec.Print();
      // myTCand->setTrackSeed(posSeed,vec,-1);
      // ///-------------------------------------
      

      tofill.push_back(*(myTCand)); //save Track Candidate
      //new((*fTrackCandArray)[trackCnt]) PndTrackCand(*(myTCand)); 
      delete myTCand;
    }//Track Cand build
  }//end of pseudo-tracks

}
*/
// -------------------------------------------------------------------------

// -----   Public method Exec   --------------------------------------------
/*void PndMQStraightLineTrackFinder::Exec(Option_t* opt)
{
  TStopwatch *timer_exec = new TStopwatch();
   if(fVerbose>2) timer_exec->Start();
  if(fVerbose>2) cout << "Evt " << FairRootManager::Instance()->GetEntryNr() << " started--------------"<<endl<<endl;

  //which combinations of planes to build pseudo-vectos
  //Int_t planeFlag=0; //flag=... 0: planes 1-2,   1: 1-2 & 1-3,   2: 1-2 & 1-3 & 2-3

  // Reset output array
  if ( ! fTrackCandArray )
    Fatal("Exec", "No trackCandArray");
  fTrackCandArray->Clear();

  Int_t nStripHits = fHitArray->GetEntriesFast();
  if(fVerbose>2) cout << "# Hits: \t"<< nStripHits <<endl<<endl;
  bool usedFlag[nStripHits]; //for pseudo-vector building
  for(Int_t seti=0; seti<nStripHits; seti++)
    usedFlag[seti]=false;

  if(nStripHits<2){
    if(fVerbose>2) cout << "Evt finsihed: too less hits-----"<<endl<<endl;
    return;
  }

  //-----do some sorting first----------
  std::vector< std::vector< std::pair<int,bool> > > hitsd(4);
 bool resSortHits;
 if(flagStipSens) resSortHits = SortHitsByDet(hitsd, nStripHits);//!strip sensors
 else{
   if(flagPixelSens) resSortHits = SortHitsByDet2(hitsd, nStripHits);//! pixel sensors
   else{
     std::cout<<"Algorithm is needed sensor type! Please, set it via SetSensStripFlag(bool fS) or SetSensPixelFlag(bool fS)"<<std::endl;
     return;
   }
 }
 if(!resSortHits){
   if(fVerbose>2) cout << "Evt finsihed: too less planes-----"<<endl<<endl;
   return;
 }


  if(fVerbose>2){
    cout<<"HitMap size: "<< hitsd.size() <<endl;
    if(hitsd.size()>0){
      for (Int_t i=0; i<hitsd.at(0).size(); i++){
	PndSdsHit *hit=(PndSdsHit*)fHitArray->At(hitsd.at(0).at(i).first);
	cout<<"Plane0 Hit=("<<hit->GetX()<<", "<<hit->GetY()<<", "<<hit->GetZ()<<")"<<endl;
      }
    }
    if(hitsd.size()>1){
      for (Int_t i=0; i<hitsd.at(1).size(); i++){
	PndSdsHit *hit=(PndSdsHit*)fHitArray->At(hitsd.at(1).at(i).first);
	cout<<"Plane1 Hit=("<<hit->GetX()<<", "<<hit->GetY()<<", "<<hit->GetZ()<<")"<<endl;
      }
    }
    if(hitsd.size()>2){
      for (Int_t i=0; i<hitsd.at(2).size(); i++){
	PndSdsHit *hit=(PndSdsHit*)fHitArray->At(hitsd.at(2).at(i).first);
	cout<<"Plane2 Hit=("<<hit->GetX()<<", "<<hit->GetY()<<", "<<hit->GetZ()<<")"<<endl;
      }
    }
    if(hitsd.size()>3){
      for (Int_t i=0; i<hitsd.at(3).size(); i++){
	PndSdsHit *hit=(PndSdsHit*)fHitArray->At(hitsd.at(3).at(i).first);
	cout<<"Plane3 Hit=("<<hit->GetX()<<", "<<hit->GetY()<<", "<<hit->GetZ()<<")"<<endl;
      }
    }
  }

  std::vector<PndTrackCand> theCands; //Track Candidates

  //--------find some tracks--------------
  FindHitsI(theCands, hitsd, nStripHits);
  if(fFinderMode){ 
    FindHitsII(theCands, hitsd, nStripHits);
    FindHitsIII(theCands, hitsd, nStripHits);
  }

  //fill tracklist für fitting
  for(int t=0; t<theCands.size(); t++)
    new((*fTrackCandArray)[t]) PndTrackCand(theCands.at(t)); 

  if(fVerbose>2) cout << "Evt finsihed--------------"<<endl<<endl;
  if(fVerbose>2){
    timer_exec->Stop();
    Double_t rtime_exec = timer_exec->RealTime();
    Double_t ctime_exec = timer_exec->CpuTime();
    cout << "Real time for Exec:" << rtime_exec << " s, CPU time " << ctime_exec << " s" << endl;
    cout << endl;
  }
}
*/
// -------------------------------------------------------------------------

Double_t PndMQStraightLineTrackFinder::GetTrackCurvature(PndMCTrack* myTrack)
{
  TVector3 p = myTrack->GetMomentum();
  return (2/TMath::Sqrt(p.Px()*p.Px() + p.Py()*p.Py()));
}
// -------------------------------------------------------------------------

Double_t PndMQStraightLineTrackFinder::GetTrackDip(PndMCTrack* myTrack)
{
  TVector3 p= myTrack->GetMomentum();
  return (p.Mag()/TMath::Sqrt(p.Px()*p.Px() + p.Py()*p.Py()));
}