//-----------------------------------------------------------
// File and Version Information:
// $Id$
//
// Description:
//      Implementation of class TpcLaserMatchingTask
//     
// Environment:
//      Software developed for the GEM-TPC detector
//
// Author List:
//	Felix Boehmer    	TUM     (original author)
//
//-----------------------------------------------------------



#include "TpcLaserMatchingTask.h"

#include "TpcLaserGrid.h"
#include "TpcLaser.h"
#include "GFAbsTrackRep.h"
#include "GFTrack.h"
#include "GFTrackCand.h"
#include "TpcCluster.h"
#include "FairRootManager.h"
#include "TClonesArray.h"
#include "TpcDigiPar.h"
#include "TpcDigiMapper.h"
#include "FairRun.h"
#include "FairRuntimeDb.h"
#include "TpcResidual.h"
#include "TpcSPHit.h"
#include "GFRecoHitFactory.h"
#include "GFRecoHitProducer.h"
#include "GFException.h"

#include "TVector3.h"

#include <map>


TpcLaserMatchingTask::TpcLaserMatchingTask()		//default constructor
  : FairTask("TPC Laser Matching"),
    fPersistence(false),
    fVerbose(false),
    fPreSelect(false),
    fClusterBranchName("TpcCluster"),
    fResidualBranchName("TpcLaserMatchingResidual"),
    fMaxDist(1.5),
    fMatched(0),
    fNotMatched(0)
{
  fFactory = new GFRecoHitFactory();
}



TpcLaserMatchingTask::~TpcLaserMatchingTask()
{
  delete fFactory;
}

InitStatus
TpcLaserMatchingTask::Init()
{

 //Get ROOT Manager
  FairRootManager* ioman= FairRootManager::Instance();
  if(ioman==0)
    {
      Error("TpcLaserMatchingTask::Init","RootManager not instantiated!");
      return kERROR;
    }

  // Get input collection
  fClusterArray=(TClonesArray*) ioman->GetObject(fClusterBranchName);
  
  if(fClusterArray==0)
    {
      Error("TpcLaserMatchingTask::Init","TpcCluster-Array not found!");
      return kERROR;
    }
  
  //read in parameters
  fZMin = fPar->windowMin();
  fZMax = fPar->windowMax();
  fRMin=fPar->getRMin();
  fRMax=fPar->getRMax();
   
  // create and register output array
  fGFTrackArray = new TClonesArray("GFTrack"); 
  ioman->Register("TpcGFLaserTrack","Tpc",fGFTrackArray,fPersistence);

  if(fVerbose) {
    fResidualArray = new TClonesArray("TpcResidual");
    ioman->Register(fResidualBranchName,"Tpc",fResidualArray,fPersistence);
  }
  
  std::cout<<"************* TpcLaserMatchingTask::Init() *************\n"
	   <<"  Initialized with fMaxDist = "<<fMaxDist<<std::endl
	   <<"********************************************************\n"
	   <<std::endl;
  
  return kSUCCESS;
}


void 
TpcLaserMatchingTask::SetParContainers() {

  std::cout<<"TpcLaserMatchingTask::SetParContainers"<<std::endl;
  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");
}



void
TpcLaserMatchingTask::Exec(Option_t* opt) 
{
  int clBranchID = FairRootManager::Instance()->GetBranchId(fClusterBranchName);
  
  fMatched = 0;
  fNotMatched = 0;
  if(fVerbose && fGFTrackArray==0) Fatal("TpcLaserMatchingTask::Exec()","No Output Array!");
  if(fVerbose)
    fGFTrackArray->Delete();
  
  //ATTENTION: Assumption is that lasers have cont. ID from 0 to maxID
  //then the ID in the map is equal to the TClonesArray index
  const std::map<unsigned int, GFAbsTrackRep*>* repMap = TpcLaserGrid::Instance()->GetTracks();
  const std::map<unsigned int, TpcLaser*>* lasMap = TpcLaserGrid::Instance()->GetLasers();

  std::map<unsigned int, GFTrackCand*> candMap;
  std::cout<< "\nTpcLaserMatchingTask::Exec(): Starting Laser Matching ..."<<std::endl;
  
  std::map<unsigned int, TpcCluster*> matchMap;  //unamb. association Track <-> Cluster
  unsigned int nCl = fClusterArray->GetEntriesFast();

  //loop over clusters
  for(unsigned int iCl=0; iCl<nCl; iCl++) {
    //calculate distance from track and associate to closest track.
    TpcCluster* cl = (TpcCluster*) fClusterArray->At(iCl);
    double minDist = fMaxDist;
    unsigned int bestID = 0;  //best matching laser track ID
    bool matched = false;
    //loop over tracks
    std::map<unsigned int, TpcLaser*>::const_iterator it;
    
    //memorize the best match for residual dumping
    TVector3 bestRes, bestpP;
        
    for(it=lasMap->begin(); it!=lasMap->end(); it++) {
      unsigned int iLs = it->first;  
      TpcLaser* ilaser = it->second;
      TVector3 dir =  ilaser->getEnd() - ilaser->getStart();
      dir.SetMag(1.0);
          
      // This test is no longer valid. 
      // Laser objects in the grad have never been "activated" during 
      // reconstruction macro
      
      // if(ilaser->getNE() < 10)
      // 	continue;
      
      
      TVector3 relPos = cl->pos() - ilaser->getStart(); //position of cluster relative to beam start
      double projection = dir*relPos;

      if(projection<0)
	continue;
      
      TVector3 pP = (projection * dir) + ilaser->getStart();
      TVector3 res = cl->pos() - pP;
      if(res.Mag() < fMaxDist) {
	matched = true;
	if(fVerbose && fPreSelect) { //write out residual
	  unsigned int nres = fResidualArray->GetEntriesFast();
	  TpcResidual* resi = new ((*fResidualArray)[nres]) TpcResidual();
	  resi->setResXYZ(res);
	  resi->setRefPos(pP);
	  resi->setHitPos(cl->pos());
	  resi->setHitIndex(iCl);
	}
	if(res.Mag() < minDist) {
	  bestID = iLs;
	  minDist = res.Mag();
	  bestRes = res;
	  bestpP = pP;
	}
      }
      
	
    } //end loop over lasers
    if(matched) 
      fMatched++;
    else
      fNotMatched++;
    if(fVerbose && fPreSelect==false) { //write out residual
      unsigned int nres = fResidualArray->GetEntriesFast();
      TpcResidual* resi = new ((*fResidualArray)[nres]) TpcResidual();
      resi->setResXYZ(bestRes);
      resi->setRefPos(bestpP);
      resi->setHitPos(cl->pos());
      resi->setHitIndex(iCl);
    }
    
    //associate cluster to lasertrack
    if(matched) {
      if(!candMap.count(bestID)) 
	candMap[bestID] = new GFTrackCand();
      (candMap[bestID])->addHit(clBranchID,iCl);
    }
  }//end loop over clusters
  
  //build GFTrack objects:
  //each laser will get a GFTrack, TCA index is assumed to be the laser ID
  //if there were no successful associations the GFTrack will stay empty
  std::cout<<"TpcLaserMatchingTask::Exec(): \n"	
	   <<"   Initialising GFTracks with GFRecoHitFactory ..."<<std::endl;
  
  fFactory->addProducer(FairRootManager::Instance()->GetBranchId(fClusterBranchName),
			new GFRecoHitProducer<TpcCluster,TpcSPHit>(fClusterArray));
  
  std::map<unsigned int, GFAbsTrackRep*>::const_iterator repit;
  for(repit=repMap->begin(); repit!=repMap->end(); repit++) {
    GFAbsTrackRep* rep = repit->second;
    GFTrack* trk = new ((*fGFTrackArray)[repit->first]) GFTrack(rep);
    if(candMap.count(repit->first)) {
      trk->setCandidate(*candMap[repit->first]);    
      try {
	trk->addHitVector(fFactory->createMany(trk->getCand()));
      }
      catch(GFException& e) {
	std::cout << e.what() << std::endl;
	e.info();
      }
    }
  }
  
  //clean up
  std::map<unsigned int, GFTrackCand*>::iterator cit;
  for(cit=candMap.begin(); cit!=candMap.end(); cit++)
    delete cit->second;

  
  std::cout<<"TpcLaserMatchingTask::Exec(): \n"	
	   <<"   Out of "<<fClusterArray->GetEntriesFast()<<" TpcClusters:\n"
	   <<"   Matched:      "<<fMatched<<std::endl
	   <<"   Not Matched:  "<<fNotMatched<<"\n"<<std::endl;
  
}