//-----------------------------------------------------------
// 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 "TpcPoint.h"
//#include "DemoRecoHit.h"
#include "CbmMCPoint.h"
#include "LSLTrackRep.h"
#include "Kalman.h"
#include "FitterExceptions.h"
#include "AbsBFieldIfc.h"
#include "CbmFieldAdaptor.h"
#include "CbmRunAna.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;
    }
 
  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
  
  // Get input collection
  //_pointArray=(TClonesArray*) ioman->GetObject(_pointBranchName);
  
  //if(_pointArray==0)
  //  {
      //    Error("DemoPatternRecoTask::Init","Cluster-array not found!");
  //    return kERROR;
  //  }
  
  // create and register output array
  _trackArray = new TClonesArray("Track"); 
  ioman->Register("TrackPreFit","GenFit",_trackArray,_persistence);
 
  //_field=CbmRunAna::Instance()->GetField();
  if(_field==NULL){
    Error("DemoPatternRecoTask::Init","BField not found!");
    return kERROR;
  }

  //Info("DemoPatternRecoTask::Init","Bz(10,10,10)=%f",_field->GetBz(10,10,10));
  _fieldIfc=new CbmFieldAdaptor(_field);

  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;
     }

     // create track object
     LSLTrackRep* rep=new LSLTrackRep();
     rep->SetBField(_fieldIfc);
     Track* trk=new((*_trackArray)[_trackArray->GetEntriesFast()]) Track(rep);
     trk->setCandidate(*cand); // here the candidate is copied! 
                               //Is this what we want?
     // calcualte start values
     unsigned int detID;
     unsigned int hitID;
     cand->getHit(0,detID,hitID);
     CbmMCPoint* cl1=(CbmMCPoint*)_hitBranchMap[detID]->At(hitID);
     cand->getHit(1,detID,hitID);
     CbmMCPoint* cl2=(CbmMCPoint*)_hitBranchMap[detID]->At(hitID);
     TVector3 pos1;cl1->Position(pos1);
     TVector3 pos2;cl2->Position(pos2);
     TVector3 delta=pos2-pos1;
     if(delta.Z()==0){
       std::cout<<"Track initialization went wrong dz==0"<<std::endl;
        ++candIter;
	trk->getTrackRep(0)->setStatusFlag(2);
       continue;
     }
     double mx=delta.X()/delta.Z();
     double my=delta.Y()/delta.Z();
     std::cout<<"mx="<<mx<<"  my="<<my<<std::endl;
     TMatrixT<double> state(5,1);
     state[0][0]=pos1.X();
     state[1][0]=pos1.Y();
     state[2][0]=mx;
     state[3][0]=my;
     state[4][0]=2.; // p=500MeV;
     trk->getTrackRep(0)->setState(state);
     trk->getTrackRep(0)->setStartState(state);
     TMatrixT<double> cov(5,5);
     cov[0][0]=100;
     cov[1][1]=100;
     cov[2][2]=16;
     cov[3][3]=16;
     cov[4][4]=5;
     trk->getTrackRep(0)->setCov(cov);
     trk->getTrackRep(0)->setStartCov(cov);
     DetPlane pl(pos1+TVector3(0,0,-10E-4),TVector3(1,0,0),TVector3(0,1,0));
     trk->getTrackRep(0)->setReferencePlane(pl);
     trk->getTrackRep(0)->setStartS(pos1.Z()-10E-4);
     
     ++candIter;
   }// end loop over tracks
   
   std::cout<<_trackArray->GetEntriesFast()<<" tracks created"<<std::endl;

   return;
}

ClassImp(DemoPatternRecoTask)