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

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

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

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

using std::cout;
using std::endl;

// Collaborating Class Headers --------
#include "TClonesArray.h"
#include "Track.h"
#include "TrackCand.h"
#include "Kalman.h"
#include "TpcConfMapRecoHit.h"
#include "TpcConfMapFit.h"
#include "TpcZSFit.h"
#include "TpcClusterRadius.h"

// Class Member definitions -----------
void 
TpcConfTrackFinder::configure(double xcut, double ycut, double zcut,
			      double chi2cut,
			      unsigned int minHitsForFit,
			      bool domerge,
			      double radiusMergeCut,  // fractional cut
			      double dipMergeCut,     // fractional cut
			      double distMergeCut)
{
  _xcut=xcut;_ycut=ycut;_zcut=zcut;
  _minHitsForFit=minHitsForFit;
  _chi2cut=chi2cut;
  _radiusMergeCut=radiusMergeCut;  // fractional cut
  _dipMergeCut=dipMergeCut;
  _distMergeCut=distMergeCut;
  _domerge=domerge;
}

void
TpcConfTrackFinder::buildTracks(TClonesArray* clusterarray,
				std::vector<TrackCand*>& candlist)
{
  throw;
}

void
TpcConfTrackFinder::buildTracks(std::vector<TpcCluster*>& cll,
				std::vector<TrackCand*>& candlist)
{
  std::cout<<"TpcConfTrackFinder::buildTracks:: starting."<<std::endl;
  std::sort(cll.begin(),cll.end(),TpcClusterRadius());
  unsigned int ncl=cll.size();
  std::vector<Track*> trks; // all tracks
  
  for(unsigned int icl=0;icl<ncl;++icl){//loop over clusters from large radius inwards
    
    TpcConfMapRecoHit* hit=new TpcConfMapRecoHit(cll[icl]);
    bool foundProx=false;
    bool foundConf=false;

    unsigned int bestHitMatchIndex=0;
    double bestHitMatchQuality=999;
    unsigned int bestConfMatchIndex=0;
    double bestConfMatchQuality=999;
    
    unsigned int ntrk=trks.size();
    for(unsigned int itrk=0;itrk<ntrk;++itrk){//loop over track candidates
      double hitMatchQuality;
      double confMatchQuality;
      unsigned int _maxhitsintrack=1000;
      if(trks[itrk]->getNumHits()>=_maxhitsintrack)continue;
      if(trks[itrk]->getNumHits()<_minHitsForFit){
	foundProx|=matchHitTrack(hit,trks[itrk],hitMatchQuality);
	if(hitMatchQuality<bestHitMatchQuality){
	  bestHitMatchQuality=hitMatchQuality;
	  bestHitMatchIndex=itrk;
	}
      }
      else{
	foundConf|=matchConfTrack(hit,trks[itrk],confMatchQuality);
	if(confMatchQuality<bestConfMatchQuality){
	  bestConfMatchQuality=confMatchQuality;
	  bestConfMatchIndex=itrk;
	}
      }

      } // end loop over track candidates
    

    if(!foundProx && !foundConf){//create new track candidate
      //std::cout<<"create new track"<<std::endl;
      //std::cout<<"hitMatchQuality="<<bestHitMatchQuality<<std::endl;
      //std::cout<<"confMatchQuality="<<bestConfMatchQuality<<std::endl;
      Track* newtrk=new Track(new TpcConfMapFit());
      AbsTrackRep* zsfit=new TpcZSFit();
      newtrk->addTrackRep(zsfit);
      trks.push_back(newtrk);
      addHit2Track(hit,newtrk);
    }
    else {// add hit to bestMatch
      if(foundConf)addHit2Track(hit,trks[bestConfMatchIndex]);
      else addHit2Track(hit,trks[bestHitMatchIndex]);
    }
  }// end loop over clusters
  
  // merge track pieces
  if(_domerge)merge(trks); // be carefull this might delete tracks!!!

  // output of TrackCandidates
  unsigned int nfound=trks.size();
  std::cout<<nfound<<" tracks found"<<std::endl;
  for(unsigned int i=0;i<nfound;++i){
    if(trks[i]==NULL)continue;
    if(trks[i]->getNumHits()>_minHitsForFit){
      double r=((TpcConfMapFit*)trks[i]->getTrackRep(0))->getR();
      double d=((TpcZSFit*)trks[i]->getTrackRep(1))->getDip();
      //std::cout<<"Track with radius r="<<r<<std::endl;
      //std::cout<<"Hits in cand="<<trks[i]->getCand().getNHits()<<std::endl;
      TrackCand* cand=new TrackCand(trks[i]->getCand());
      if(r==0)r=1E-12;
      cand->setCurv(1./r);
      cand->setDip(d);
      candlist.push_back(cand);
    }
    delete trks[i];
  }
  trks.clear();
  std::cout<<candlist.size()<<" candidates of appropriate length"<<std::endl;
  std::cout<<"TpcConfTrackFinder::buildTracks:: finished."<<std::endl;
}


bool
TpcConfTrackFinder::matchHitTrack(TpcConfMapRecoHit* hit,
				  Track* trk,
				  double& matchQuality)
{
  unsigned int nhitsInTrk=trk->getNumHits();
  bool alive=true;
  TpcConfMapRecoHit* hitontrk=dynamic_cast<TpcConfMapRecoHit*>(trk->getHit(nhitsInTrk-1)); // last hit
  alive=applyProximityCuts(hit,hitontrk);
  matchQuality=hitDist(hit,hitontrk);
  return alive;
}


bool
TpcConfTrackFinder::matchConfTrack(TpcConfMapRecoHit* hit,
				  Track* trk,
				  double& matchQuality)
{
  hit->setRotated(dynamic_cast<TpcConfMapFit*>(trk->getTrackRep(0))->isRotated());
  Kalman myfitter;
  double ConfMatchQuality=myfitter.getChi2Hit(hit,trk->getTrackRep(0));
  double ZSmatchQuality=999.;
  if(ConfMatchQuality>_chi2cut){
    //std::cout<<"bad match: "<<matchQuality<<std::endl;
    matchQuality=ConfMatchQuality;
    return false;
  }
  else {
    // do zs-check
    // set s for this candidate
    hit->calc_s(dynamic_cast<TpcConfMapFit*>(trk->getTrackRep(0))->getR());
    ZSmatchQuality=myfitter.getChi2Hit(hit,trk->getTrackRep(1));
    if(ZSmatchQuality>_chi2cut){
      matchQuality=ZSmatchQuality;
      return false;
    }
  }
  //std::cout<<"ZSmatchQuality="<<ZSmatchQuality<<std::endl;
  //std::cout<<"ConfMatchQuality="<<ConfMatchQuality<<std::endl;
  matchQuality=TMath::Max(ConfMatchQuality,ZSmatchQuality);
  //std::cout<<"matchConfTrack::chi2="<<matchQuality<<std::endl;
  return true;
}

void
TpcConfTrackFinder::merge(std::vector<Track*>& trks)
  // careful: will delete tracks!!!
{
  //std::cout<<"Beginning track merge"<<std::endl;
  // for the moment do the combinatorics brute force
  unsigned int ntrks=trks.size();
  if(ntrks<2)return;
  for(unsigned int i=0;i<ntrks;++i){
    Track* thetrk=trks[i];
    if(thetrk==NULL) continue;
    double r=dynamic_cast<TpcConfMapFit*>(thetrk->getTrackRep(0))->getR();
    double dip=dynamic_cast<TpcZSFit*>(thetrk->getTrackRep(1))->getDip();
    TpcConfMapRecoHit* hit1a=dynamic_cast<TpcConfMapRecoHit*>(thetrk->getHit(0));
    TpcConfMapRecoHit* hit1b=dynamic_cast<TpcConfMapRecoHit*>(thetrk->getHit(thetrk->getNumHits()-1));
    for(int j=i+1;j<ntrks;++j){
      Track* atrk=trks[j];
      if(atrk==NULL)continue;
      // check radius
      double ar=dynamic_cast<TpcConfMapFit*>(atrk->getTrackRep(0))->getR();
      if(fabs(ar-r)>_radiusMergeCut*r){cout<<"radiuscut"<<endl;continue;}
      // check dip
      double adip=dynamic_cast<TpcZSFit*>(atrk->getTrackRep(1))->getDip();
      if(fabs(adip-dip)>_dipMergeCut*dip){cout<<"dipcut"<<endl;continue;}
      // check proximity
      TpcConfMapRecoHit* hit2a=dynamic_cast<TpcConfMapRecoHit*>(atrk->getHit(0));
      TpcConfMapRecoHit* hit2b=dynamic_cast<TpcConfMapRecoHit*>(atrk->getHit(atrk->getNumHits()-1));
      // check beginning - end
      // check end - beginning
      double dist1=hitDist(hit1a,hit2b);
      double dist2=hitDist(hit1b,hit2a);
      double checkquality;
      bool check1=matchConfTrack(hit2a,thetrk,checkquality);
      bool check2=matchConfTrack(hit2b,thetrk,checkquality);
      if(!check1 && !check2){cout<<"confcut"<<endl;continue;}
      // if survived so far -> merge!
      if(dist1<dist2){
	mergeTracks(atrk,thetrk); // thetrk will disappear
	delete thetrk;trks[i]=NULL;
      }
      else {
	mergeTracks(thetrk,atrk);
	delete atrk; trks[j]=NULL;
      }
      //std::cout<<"Tracks merged!"<<std::endl;
      if(trks[i]==NULL)break; // break inner look oer tracks
    } // end inner loop over trks
  }// end outer loop over trks
}

void
TpcConfTrackFinder::mergeTracks(Track* trkA,Track* trkB)
  // trkB will be deleted!
{
  Kalman myfitter;
  trkA->mergeHits(trkB); // thetrk will disappear
  myfitter.continueTrack(trkA);
}

// ---------------------------------------------------------------------
// --------- Helper Functions ------------------------------------------
// ---------------------------------------------------------------------

bool
TpcConfTrackFinder::applyProximityCuts(TpcConfMapRecoHit* hit,
				       TpcConfMapRecoHit* hitontrk)
{
  bool alive=true;
  if(fabs(hitontrk->z()-hit->z())>_zcut){alive=false;}
  if(alive){if(fabs(hitontrk->x()-hit->x())>_xcut){alive=false;}}
  if(alive){if(fabs(hitontrk->y()-hit->y())>_ycut){alive=false;}}
  return alive;
}

double
TpcConfTrackFinder::hitDist(TpcConfMapRecoHit* hit1,
			    TpcConfMapRecoHit* hit2)
{
  double dz=hit2->z()-hit1->z();
  double dx=hit2->x()-hit1->x();
  double dy=hit2->y()-hit1->y();
  double d=sqrt(dz*dz+dx*dx+dy*dy);
  return d;
}

bool
TpcConfTrackFinder::addHit2Track(TpcConfMapRecoHit* hit,
				 Track* trk)
{
  hit->calc_s(dynamic_cast<TpcConfMapFit*>(trk->getTrackRep(0))->getR());
  // here is a problem because we cannot calculate s meaningfull 
  // if the fit has not been performed in conf-space first!
  //std::cout<<"adding hit to trk at position "<<trk->getNumHits()<<std::endl;
  trk->addHit(hit,2,hit->index());

  if(trk->getNumHits()==_minHitsForFit){ // first round of fits
    Kalman myfitter;
    // for a first pass turn off zs-fit:
    trk->getTrackRep(1)->setStatusFlag(13);
    // set starting values
    TpcConfMapRecoHit* chit1=dynamic_cast<TpcConfMapRecoHit*>(trk->getHit(0));
    TpcConfMapRecoHit* chit2=dynamic_cast<TpcConfMapRecoHit*>(trk->getHit(1));
    double dyp=chit2->getHitCoord(DetPlane())[0][0]-chit1->getHitCoord(DetPlane())[0][0];
    double dxp=chit2->getXcf()-chit1->getXcf();
    if(dxp==0)dxp=1E-12;
    double m=dyp/dxp;
    // check wether m is not too large!
    // if so -> rotate this track!
    if(fabs(m)>10.){// rotate problem
      (dynamic_cast<TpcConfMapFit*>(trk->getTrackRep(0)))->setRotated();
      m=-1./m;
      // rotate all hits in trk!
      for(unsigned int i=0;i<_minHitsForFit;++i){
	TpcConfMapRecoHit* conhit=dynamic_cast<TpcConfMapRecoHit*>(trk->getHit(i));
	conhit->setRotated(dynamic_cast<TpcConfMapFit*>(trk->getTrackRep(0))->isRotated());
      }
    }
    
    double t=chit1->getHitCoord(DetPlane())[0][0]-m*chit1->getXcf();
    TMatrixT<double> state=trk->getTrackRep(0)->getState();
    state[0][0]=m;
    state[1][0]=t;
    trk->getTrackRep(0)->setState(state);
    myfitter.processTrack(trk);
    // now reset s for the first hits
    for(unsigned int i=0;i<_minHitsForFit;++i)
      {
	TpcConfMapRecoHit* conhit=dynamic_cast<TpcConfMapRecoHit*>(trk->getHit(i));
	conhit->calc_s(dynamic_cast<TpcConfMapFit*>(trk->getTrackRep(0))->getR());
      }
    // for second pass turn off confMapFit,turn on zs-fit
    trk->getTrackRep(0)->setStatusFlag(13);
    trk->getTrackRep(1)->setStatusFlag(0);
    myfitter.processTrack(trk);
    // turn on both fits again
    trk->getTrackRep(0)->setStatusFlag(0);
  }
  else if(trk->getNumHits()>_minHitsForFit){ // regular fits
    hit->setRotated((dynamic_cast<TpcConfMapFit*>(trk->getTrackRep(0))->isRotated()));
    Kalman myfitter;
    myfitter.continueTrack(trk);
  }
  
  return true;
  // here some more adminstrative stuff has to go
}