//-----------------------------------------------------------
// File and Version Information:
// $Id$
//
// Description:
//      Implementation of class PndTpcRiemannTrackFinder
//      see PndTpcRiemannTrackFinder.h for details
//
// Environment:
//      Software developed for the PANDA Detector at FAIR.
//
// Author List:
//      Sebastian Neubert    TUM            (original author)
//
//
//-----------------------------------------------------------

// Panda Headers ----------------------

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

// C/C++ Headers ----------------------
#include <algorithm>
#include <iostream>

// Collaborating Class Headers --------
#include "DebugLogger.h"
#include "PndTpcClusterRadius.h"
#include "PndTpcClusterZ.h"
#include "TrackCand.h"
#include "PndTpcCluster.h"
#include "PndTpcRiemannTrack.h"
#include "PndTpcRiemannHit.h"
#include "PndTpcProximityHTCorrelator.h"
#include "PndTpcRiemannHTCorrelator.h"

// Class Member definitions -----------


PndTpcRiemannTrackFinder::PndTpcRiemannTrackFinder()
  : _minHitsForFit(5)
{
  // correlators in decreasing priority!
  
  //addCorrelator(new PndTpcProximityHTCorrelator(1.));
  //addCorrelator(new PndTpcRiemannHTCorrelator(5.E-4));
}


PndTpcRiemannTrackFinder::~PndTpcRiemannTrackFinder()
{
  for(int i=0;i<_correlators.size();++i){
    if(_correlators[i]!=NULL){
      delete _correlators[i];
      _correlators[i]=NULL;
    }
  }
  _correlators.clear();

}

void
PndTpcRiemannTrackFinder::addCorrelator(PndTpcAbsHitTrackCorrelator* c)
{
  _correlators.push_back(c);
  _found.push_back(false);
  _bestMatchQuality.push_back(9999);
  _bestMatchIndex.push_back(0);
}



unsigned int
PndTpcRiemannTrackFinder::buildTracks(std::vector<PndTpcCluster*>& cll,
				   std::vector<PndTpcRiemannTrack*>& candlist)
{
  std::sort(cll.begin(),cll.end(),PndTpcClusterZ());
  unsigned int ncl=cll.size();

  for(unsigned int icl=0;icl<ncl;++icl){ // loop over clusters
    //if(icl<30)cll[icl]->pos().Print();
    PndTpcRiemannHit* rhit=new PndTpcRiemannHit(cll[icl]);
    unsigned int ntrks=candlist.size();
    
    unsigned int method=0;
    bool foundAtAll=false;
    //std::cout<<"hit"<<icl<<" testing "<<ntrks<<" tracks"<<std::endl;
    for(unsigned int itrk=0;itrk<ntrks;++itrk){ // loop over tracks
      PndTpcRiemannTrack* trk=candlist[itrk];
      if(trk==NULL)continue;
      // CHOOSE METHOD
      if(trk->getNumHits()<_minHitsForFit)method=1;
      else method=0;
      
      // CORRELATE HIT WITH TRACK
      double matchQuality=99999;
      bool found=_correlators[method]->corr(trk,rhit,matchQuality);
      //std::cout<<"method="<<method
      //	       <<" quality="<<matchQuality
      //	       <<" found="<<found<<std::endl;
      foundAtAll|=found;

      // ANALYZE
      bool f=_found[method];
      f|=found;
      _found[method]=f;
      if(_bestMatchQuality[method]>matchQuality){
	_bestMatchQuality[method]=matchQuality;
	_bestMatchIndex[method]=itrk;
      }
      
    } // end loop over tracks

    if(!foundAtAll)// new track
      {
	PndTpcRiemannTrack* trk=new PndTpcRiemannTrack();
	candlist.push_back(trk);
	//std::cout<<"Creating new track"<<std::endl;
	trk->addHit(rhit);
      }
    else {
      // which method?
      method=0;
      while(!_found[method])++method; // if first method has found use this ..
      // add track to best match
      PndTpcRiemannTrack* theTrk=candlist[_bestMatchIndex[method]];
      theTrk->addHit(rhit);
      if(theTrk->getNumHits()>=_minHitsForFit)theTrk->refit();
    }
    resetFlags();
  } // end loop over cluster
  

  std::cout<<candlist.size()<<" Riemann Tracks found."<<std::endl;
  for(int i=0;i<candlist.size();++i){
    std::cout<<"Track"<<i<<": "
	     <<candlist[i]->getNumHits()<<" hits  R=";
    if(candlist[i]->getNumHits()>=_minHitsForFit)std::cout<<candlist[i]->r();
    std::cout<<std::endl;
  }
  
 return candlist.size();
}

void
PndTpcRiemannTrackFinder::resetFlags(){
// reset all flags
  for(int k=0;k<_correlators.size();++k){
    _found[k]=false;
    _bestMatchQuality[k]=99999;
    _bestMatchIndex[k]=0;
  }
}