/***************************************************************************
                         InDetSoftwareTutorial.cxx  -  Description
                             -------------------
    begin   : 10-05-2005
    authors : Edward Moyse(CERN PH-ATC)
    email   : Edward.Moyse@cern.ch
 ***************************************************************************/

#include "GaudiKernel/MsgStream.h"

#include "StoreGate/StoreGateSvc.h"

#include "CLHEP/Matrix/Vector.h"

#include "TrkTrack/Track.h"
#include "TrkTrack/TrackCollection.h"
#include "TrkParameters/Perigee.h"

#include "InDetSoftwareTutorial/InDetSoftwareTutorial.h"

namespace InDet
{
  
InDetSoftwareTutorial::InDetSoftwareTutorial(const std::string &n, ISvcLocator *pSvcLoc) 
  : 
  Algorithm(n, pSvcLoc),
  m_fitTool(0),
  m_maxD0(0.25),
  m_storeGate(0),
  m_tracksName("Tracks") // set sensible defaults, so you don't rely on jobOptions
{
  declareProperty("TracksName",m_tracksName);
  declareProperty("maxD0", m_maxD0);
}

InDetSoftwareTutorial::~InDetSoftwareTutorial() {}

StatusCode InDetSoftwareTutorial::initialize()
{
  MsgStream log(msgSvc(), name());
  StatusCode sc;

  // Retrieve StoreGate service 
  sc = service("StoreGateSvc", m_storeGate);
  if (sc.isFailure())
  {
    log << MSG::ERROR << "Could not find StoreGateSvc." << endreq;
    return sc;
  }
//  sc = toolSvc()->retrieveTool("Trk::VxFastFit",m_fitTool);
//  if (sc.isFailure()) {
//    log << MSG::FATAL 
//	    << "Could not get Trk::VxFastFit tool. Cannot run. Quitting." 
//	    << endreq;
//    return StatusCode::FAILURE;
//  } else {
//    log << MSG::VERBOSE << "Woohoo! Successfully found VxFastFit tool."<<endreq;
//  }

  
  log << MSG::INFO << "Initialization successful" << endreq;

  return StatusCode::SUCCESS;
}

StatusCode InDetSoftwareTutorial::finalize()
{
  return StatusCode::SUCCESS;
}

StatusCode InDetSoftwareTutorial::execute()
{
  MsgStream log(msgSvc(), name());
  // Retrieve tracks from StoreGate 
  const TrackCollection* tracks;
  StatusCode sc = m_storeGate->retrieve( tracks, m_tracksName);
  if (sc.isFailure())
  {
    log << MSG::DEBUG << "Could not find DataVector<Trk::Track> " << m_tracksName << " in StoreGate." << endreq;
    return StatusCode::SUCCESS; 
    // it may seem strange to return SUCCESS when in fact we failed to find any tracks
    // but the point is that returning FAILURE can cause Athena to stop 
    // (which should only happen with catasprohic errors)
  }
  log << MSG::DEBUG << "Retrieved " << tracks->size() << " tracks " << endreq;

    std::vector<const Trk::Track*> selectedTracks;
    DataVector<Trk::Track>::const_iterator trackIt    = tracks->begin();
    DataVector<Trk::Track>::const_iterator trackItEnd = tracks->end();    
    for ( ; trackIt!= trackItEnd; trackIt++)
    {
      // loop over tracks
      //DetectorDescription/GeoModel/HitDisplay/src/TrackSystem.cxx
      const Trk::MeasuredPerigee *perigee = dynamic_cast<const Trk::MeasuredPerigee *>((**trackIt).perigeeParameters());
//      const Trk::Perigee *aPerigee=*trackIt->perigeeParameters();
      const HepVector& perigeeParms = perigee->parameters();
      double d0 = perigeeParms[Trk::d0];
 
       log << MSG::DEBUG << "d0= " << d0 <<  endreq;

      if(d0 < m_maxD0) selectedTracks.push_back(*trackIt);
    }

    log << MSG::DEBUG << "Of " << tracks->size() << " tracks " << selectedTracks.size() << " survived the preselection." << endreq;

  return StatusCode::SUCCESS;
}

}//end of namespace
  
