//-----------------------------------------------------------
// File and Version Information:
// $Id$
//
// Description:
//      Implementation of class DemoPatternRecoTask
//      see DemoPatternRecoTask.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 "DemoPatternRecoTask.h"

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

// Collaborating Class Headers --------
#include "CbmRootManager.h"
#include "TClonesArray.h"
#include "Track.h"
#include "TrackCand.h"
#include "CbmMCPoint.h"
#include "CbmMCTrack.h"
#include "LSLTrackRep.h"
#include "GeaneTrackRep.h"
#include "Kalman.h"
#include "FitterExceptions.h"
#include "CbmGeanePro.h"
#include "CbmTrackParP.h"


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

DemoPatternRecoTask::DemoPatternRecoTask()
  : CbmTask("Ideal Pattern Reco"), _persistence(kFALSE)
{
}


DemoPatternRecoTask::~DemoPatternRecoTask()
{
}

InitStatus
DemoPatternRecoTask::Init()
{
  //Get ROOT Manager
  CbmRootManager* ioman= CbmRootManager::Instance();

  if(ioman==0)
    {
      Error("DemoPatternRecoTask::Init","RootManager not instantiated!");
      return kERROR;
    }
 
  // open hit arrays
  std::map<unsigned int,TString>::iterator iter=_hitBranchNameMap.begin();
  while(iter!=_hitBranchNameMap.end()){
   TClonesArray* ar=(TClonesArray*) ioman->GetObject(iter->second);
   if(ar==0){
     Error("DemoPRTask::Init","point-array %s not found!",iter->second.Data());
   }
   else{ 
	 _hitBranchMap[iter->first] = ar;
   }
  ++iter;
  }//end loops over hit types
  

  // open MCTruth array
  _mcArray=(TClonesArray*) ioman->GetObject("MCTrack");
  if(_mcArray==0){
    Error("DemoPRTask::Init","mctrack-array not found!");
    return kERROR;
  }
   
  // create and register output array
  _trackArray = new TClonesArray("Track"); 
  ioman->Register("TrackPreFit","GenFit",_trackArray,_persistence);
 
  
  // GeanePro will get Geometry and BField from the Run
  _geanePro=new CbmGeanePro();
  
  return kSUCCESS;
}


void
DemoPatternRecoTask::Exec(Option_t* opt)
{
  std::cout<<"DemoPatternRecoTask::Exec"<<std::endl;
  // Reset output Array
  if(_trackArray==0) Fatal("DemoPatternReco::Exec)","No TrackArray");
   _trackArray->Delete();

   // use McId to distinguish data from different tracks
   std::map<unsigned int,TrackCand*> candmap;


   std::map<unsigned int,TClonesArray*>::iterator iter=_hitBranchMap.begin();
   while(iter!=_hitBranchMap.end()){
	 TClonesArray* pointArray = iter->second;//_hitBranchMap[iter->first];
	 //loop over points
	 Int_t np=pointArray->GetEntriesFast();
	 for(Int_t ip=0; ip<np; ++ip){
	   CbmMCPoint* point=(CbmMCPoint*)pointArray->At(ip);
	   unsigned int id=point->GetTrackID();
	   // cut on insane ids
	   if(id>100000)continue;
	   // look for track in mc map
	   if(candmap[id]==NULL){
		 // create new track
		 std::cout<<"Creating new track candidate with id="<<id<<std::endl;
		 candmap[id]=new TrackCand;
	   }
	   // add hit to track
	   // set detectorid=2;
	   candmap[id]->addHit(iter->first,ip);
	 }// end loop over cluster
	 ++iter;
   }

   // ------------------------------------------------------------------------
   // try to find some starting values
   // loop over tracks
   
   std::map<unsigned int,TrackCand*>::iterator candIter=candmap.begin();
   while(candIter!=candmap.end()){
     TrackCand* cand=candIter->second;
     if(cand->getNHits()<10){
       ++candIter;
       continue;
     }
     
     // Get MCTrack
     CbmMCTrack* mc=(CbmMCTrack*)_mcArray->At(candIter->first);
     if(mc==0){
       Error("DemoPRTask::Exec","MCTrack Id=&i not found!",candIter->first);
       continue;
     }

     double q=TDatabasePDG::Instance()->GetParticle(mc->GetPdgCode())->Charge()/3.;
     
     TVector3 pos=mc->GetStartVertex();
     TVector3 mom=mc->GetMomentum();
     TVector3 poserr(1.,1.,1.);
     TVector3 momerr=mom*0.1; //  10% error
     momerr+=TVector3(0.01,0.01,0.01);
     
     TVector3 u(1.,0.,0.);//=mom.Orthogonal();
     //u.SetMag(1.);
     TVector3 v(0.,1.,0.);//=mom.Cross(u);
     //v.SetMag(1.);

     // create track-representation object and initialize with start values
     DetPlane pl(pos,u,v);
     GeaneTrackRep* rep=new GeaneTrackRep(_geanePro,pos,mom,
					  poserr,momerr,q,pl);

     rep->Print();

     // create track object
     Track* trk=new((*_trackArray)[_trackArray->GetEntriesFast()]) Track(rep);
     trk->setCandidate(*cand); // here the candidate is copied! 
                               
     
     ++candIter;
   }// end loop over tracks
   
   std::cout<<_trackArray->GetEntriesFast()<<" tracks created"<<std::endl;

   return;
}

ClassImp(DemoPatternRecoTask)