// Include files 
#include "STClusterTrackMonitor.h"

//event
#include "Event/Track.h"
#include "Event/TrackFitResult.h"
#include "Event/State.h"
#include "Kernel/LHCbID.h"
#include "Event/ODIN.h"
#include "Event/VeloCluster.h"
#include "Event/VeloPhiMeasurement.h"
#include "Event/VeloRMeasurement.h"
#include "Event/KalmanFitResult.h"
#include "Kernel/HitPattern.h"
#include "TrackInterfaces/IHitExpectation.h"
#include "TrackInterfaces/IVeloExpectation.h"

// Det
#include "STDet/DeSTDetector.h"
#include "STDet/DeITDetector.h"
#include "VeloDet/DeVelo.h"
#include "Event/FitNode.h"
#include "Event/RecVertex.h"


// ST DAQ
#include "Kernel/STDAQDefinitions.h"
#include "Kernel/STBoardMapping.h"
#include "Kernel/LHCbConstants.h"
#include "Kernel/ISTReadoutTool.h"
#include "Kernel/STDAQDefinitions.h"

// STTELL1Event
#include "Event/STTELL1Data.h"
#include "Kernel/STTell1Board.h"

// gsl
#include "gsl/gsl_math.h"

#include "GaudiKernel/AlgFactory.h"
#include "GaudiKernel/PhysicalConstants.h"
#include "GaudiKernel/ToStream.h"

#include <map>
#include <stdlib.h>
#include <time.h>
#include "Map.h"

// Boost
#include <boost/lambda/bind.hpp>
#include <boost/lambda/lambda.hpp>
#include <boost/foreach.hpp>
#include <boost/lexical_cast.hpp>

using namespace boost::lambda;
using namespace LHCb;
using namespace Gaudi;


DECLARE_ALGORITHM_FACTORY( STClusterTrackMonitor )

//=============================================================================
// Standard constructor, initializes variables
//=============================================================================
STClusterTrackMonitor::STClusterTrackMonitor(const std::string& name,
                           ISvcLocator* pSvcLocator ) :
  TrackMonitorTupleBase( name , pSvcLocator )
{
  declareProperty("DetType",m_detType,"TT");
  declareProperty("MaxTrChi2NDF",m_maxTrChi2NDF=5.0);
  declareProperty("ODINData",m_odinLoc,"DAQ/ODIN");

  // TT layer mapping
  m_layerMap.insert(std::pair<std::string, unsigned int>("TTaX",9));
  m_layerMap.insert(std::pair<std::string, unsigned int>("TTaU",10));
  m_layerMap.insert(std::pair<std::string, unsigned int>("TTbV",17));
  m_layerMap.insert(std::pair<std::string, unsigned int>("TTbX",18));

  // IT layer mapping
  m_layerMap.insert(std::pair<std::string, unsigned int>("T1X1",9));
  m_layerMap.insert(std::pair<std::string, unsigned int>("T1U", 10));
  m_layerMap.insert(std::pair<std::string, unsigned int>("T1V", 11));
  m_layerMap.insert(std::pair<std::string, unsigned int>("T1X2",12));

  m_layerMap.insert(std::pair<std::string, unsigned int>("T2X1",17));
  m_layerMap.insert(std::pair<std::string, unsigned int>("T2U", 18));
  m_layerMap.insert(std::pair<std::string, unsigned int>("T2V", 19));
  m_layerMap.insert(std::pair<std::string, unsigned int>("T2X2",20));

  m_layerMap.insert(std::pair<std::string, unsigned int>("T3X1",25));
  m_layerMap.insert(std::pair<std::string, unsigned int>("T3U", 26));
  m_layerMap.insert(std::pair<std::string, unsigned int>("T3V", 27));
  m_layerMap.insert(std::pair<std::string, unsigned int>("T3X2",28));
}

//=============================================================================
// Destructor
//=============================================================================
STClusterTrackMonitor::~STClusterTrackMonitor() {}; 

StatusCode STClusterTrackMonitor::initialize()
{
  // Mandatory initialization of GaudiAlgorith
  StatusCode sc = TrackMonitorTupleBase::initialize();

  
  

  if ( sc.isFailure() ) { return sc; }
 
  if (m_detType == "TT"){
    m_clusterLoc = LHCb::STClusterLocation::TTClusters;
  }else{
    m_clusterLoc = LHCb::STClusterLocation::ITClusters;
  }
  
  
  

  m_extrapolator    = tool<ITrackExtrapolator>("TrackParabolicExtrapolator");
 
  m_STDet           = getDet<DeSTDetector>(DeSTDetLocation::location(m_detType));
  
  return StatusCode::SUCCESS;
}

//=============================================================================
// Execute
//=============================================================================
StatusCode STClusterTrackMonitor::execute()
{
  

  // Create Tuple
  Tuples::Tuple m_tuple_cl = GaudiTupleAlg::nTuple("ClusterInfo/"+m_detType,   "ClusterInfo",   CLID_ColumnWiseTuple );

  // Check if ODIN is available, otherwise skip event
  bool isEverythingHere = true;

  if(!exist<LHCb::ODIN>(m_odinLoc)){ 
    warning() << "ODIN data are not available!" << endmsg;
    isEverythingHere = false;
  }

  if(!exist<LHCb::STClusters>(m_clusterLoc)){
    warning() << "Clusters are not available!" << endmsg;
    isEverythingHere = false; 
  }
  

  if(!isEverythingHere){ 
    warning() << "Skip event!" << endmsg;
    return StatusCode::SUCCESS;
  }

  // get ODIN
  const LHCb::ODIN* odin = get<LHCb::ODIN>(m_odinLoc);
  // Extract further event info
  int    runN     = odin->runNumber();
  int    evtN     = odin->eventNumber();
  debug() << "Looking at event " << evtN << " in run " << runN <<"." << endmsg;

  // get Clusters
  const LHCb::STClusters* clusters = get<LHCb::STClusters>(m_clusterLoc);
  

  // Check if track container is available,...
  if(!exist<LHCb::Track::Range>(inputContainer())){
    warning() << "Tracks are not available!" << endmsg;
    isEverythingHere = false;
  }

  // ... otherwise skip event, unless next/previous bunch crossing is excluded
  if(!isEverythingHere){ 
    warning() << "Skip event!" << endmsg;
    return StatusCode::SUCCESS;
  }

  LHCb::Track::Range tracks = get<LHCb::Track::Range>(inputContainer());
  
  
  
  LHCb::STClusters::const_iterator i_end = clusters->end();
  
  for(LHCb::STClusters::const_iterator i = clusters->begin();i!=i_end;++i){
    const LHCb::STCluster* cluster = (*i);
    const STChannelID clID  = cluster->channelID();
    unsigned int clSize     = cluster->size();
    unsigned int clMax      = cluster->maxADCValue();
    unsigned int clSta      = cluster->station();
    unsigned int clLay      = cluster->layer();
    unsigned int clDeR      = cluster->detRegion();
    unsigned int clSec      = cluster->sector();
    unsigned int clStr      = cluster->strip();

    m_layer                 = cluster->layerName();
    

    double ghostProb        = 1.0;
    double minTrChi2NDF     = m_maxTrChi2NDF;
    double trP              = -1000.0;
    double trPT             = -1000.0;
    double xHit             =     0.0;
    double yHit             =     0.0;
    double zHit             =     0.0;
    int trackType           = -1;
    int nTrack              = 0;
    

    bool foundSensor        = false;
    
    
    DeSTSector* sec         = m_STDet->findSector(clID);
    const DeSTSector::Sensors& sensors  = sec->sensors();
    BOOST_FOREACH(const DeSTSensor* sensor, sensors){
      if(sensor->contains(clID)){
        foundSensor = true;
        debug() << "Found sensor! It is "<< sensor->elementID() << endmsg;
        Gaudi::XYZPoint point = sensor->globalCentre();
        xHit = point.x();
        yHit = point.y();
        zHit = point.z();
        break;
      }    
    }
    


    BOOST_FOREACH(const LHCb::Track* track, tracks){
      // Check if it is long track
      if(track->type()!=Track::Long && 
         track->type()!=Track::Downstream &&
         track->chi2PerDoF()>m_maxTrChi2NDF &&
         track->ghostProbability()>ghostProb){
        debug()<< "No long track or too hight track chi2 or ghostProb! Skip track!" << endmsg;
        continue;
      }
      const std::vector<LHCb::LHCbID> ids = track->lhcbIDs();
      // Get extrapolated hit position
      StateVector* hit = 0;
      if(m_detType=="TT"){
        hit = GetTTHit(track);
      }else{
        hit = GetITHit(track);
      }
      if(!hit){
        warning() << "Extrapolated hit not found! Skip event!" << endmsg;
        continue;
      }
      BOOST_FOREACH(const LHCb::LHCbID id,ids){
        if(id.isST() && id.stID() == clID){
          trackType         = (int)track->type();
          trP               = track->p();
          trPT              = track->pt();
          ghostProb         = track->ghostProbability();
          minTrChi2NDF      = track->chi2PerDoF();
          // Extract hit 
          xHit = hit->x();
          yHit = hit->y();
          zHit = hit->z();
          nTrack++;
          break;
        }
      }
    }
    bool test = true;
    
    debug() << "Filling track info" << endmsg;
    test &= m_tuple_cl->column("trackType",  trackType);
    test &= m_tuple_cl->column("p",          trP);
    test &= m_tuple_cl->column("pt",         trPT);
    test &= m_tuple_cl->column("trchi2NDF",  minTrChi2NDF);
    test &= m_tuple_cl->column("ghostP",     ghostProb);
    test &= m_tuple_cl->column("nTrack",     nTrack);
    
    
    debug() << "Filling position info" << endmsg;
    test &= m_tuple_cl->column("x",        xHit);
    test &= m_tuple_cl->column("y",        yHit);
    test &= m_tuple_cl->column("z",        zHit);
    test &= m_tuple_cl->column("r",sqrt(xHit*xHit+yHit*yHit));
    test &= m_tuple_cl->column("sensFound",foundSensor);

    debug() << "Filling ODIN information" << endmsg;
    test &= m_tuple_cl->column("runNumber",runN);
    test &= m_tuple_cl->column("evtNumber",evtN);

    debug() << "Filling cluster info" << endmsg;
    test &= m_tuple_cl->column("clSize",   clSize);
    test &= m_tuple_cl->column("clMaxADC", clMax);
    test &= m_tuple_cl->column("clSta",    clSta);
    test &= m_tuple_cl->column("clLay",    clLay);
    test &= m_tuple_cl->column("clDet",    clDeR);
    test &= m_tuple_cl->column("clSec",    clSec);
    test &= m_tuple_cl->column("clStr",    clStr);

    StatusCode status = m_tuple_cl->write();
    

    if(status.isFailure() || !test){
      return Error("Cannot fill noise ntuple");
    }

    debug() << "**********************************" << endmsg;
    debug() << "Track:      p = " << trP          << " MeV/c" << endmsg;
    debug() << "           pt = " << trPT         << " MeV/c" << endmsg;
    debug() << "         type : " << trackType    << endmsg;
    debug() << "     chi2/ndf = " << minTrChi2NDF << endmsg;
    debug() << "       ghostP = " << ghostProb    << endmsg;
    debug() << "Event:    run : " << runN         << endmsg;
    debug() << "        event : " << evtN         << endmsg;
    debug() << "Cluster: size = " << clSize       << endmsg;
    debug() << "          adc = " << clMax        << endmsg;
    debug() << "      Station : " << clSta        << endmsg;
    debug() << "        Layer : " << clLay        << endmsg;
    debug() << "     # Tracks : " << nTrack       << endmsg;
    debug() << " Detector reg : " << clDeR        << endmsg;
    debug() << "       Sector : " << clSec        << endmsg;
    debug() << "        Strip : " << clStr        << endmsg;
    debug() << "Position:   x = " << xHit         << " mm" << endmsg;
    debug() << "            y = " << yHit         << " mm" << endmsg;
    debug() << "            z = " << zHit         << " mm" << endmsg;
    debug() << "            r = " << sqrt(xHit*xHit+yHit*yHit) << " mm" << endmsg;
    debug() << "**********************************" << endmsg;
  }
  


  
  debug() << "Increment counters" << endmsg;

  // fill counters....
  counter("#Tracks") += tracks.size();
  for(MultiplicityMap::const_iterator it = m_multiplicityMap.begin() ;
      it != m_multiplicityMap.end(); ++it) {
    counter("#"+it->first) += it->second;    
  }

  debug() << "End of execute!" << endmsg;
  return StatusCode::SUCCESS;
};


StateVector* STClusterTrackMonitor::GetTTHit(const LHCb::Track* track)
{

  const DeSTDetector::Stations& stations = m_STDet->stations();
  double zTTa   =  stations[0]->globalCentre().z();
  double zTTb   =  stations[1]->globalCentre().z();

  const State& TTaState = track->closestState(zTTa);
  const State& TTbState = track->closestState(zTTb);
  StateVector* stateVectorTT;

  if(std::string::npos != m_layer.find("TTa")){
    stateVectorTT = new StateVector(TTaState.position(), TTaState.slopes());
  }
  else{
    stateVectorTT = new StateVector(TTbState.position(), TTbState.slopes());
  }
  

  // loop over the sectors
  const DeSTDetector::Sectors& sectorVector = m_STDet->sectors();
  DeSTDetector::Sectors::const_iterator iterS = sectorVector.begin();
  for (; iterS != sectorVector.end(); ++iterS){
    if((*iterS)->elementID().uniqueLayer()==m_layerMap[m_layer]){
      m_extrapolator->propagate((*stateVectorTT),(*iterS)->globalCentre().z());
      return stateVectorTT;
    }
  }
  return 0;
}

StateVector* STClusterTrackMonitor::GetITHit(const LHCb::Track* track)
{
  const DeSTDetector::Stations& stations = m_STDet->stations();
  double zIT1   =  stations[0]->globalCentre().z();
  double zIT2   =  stations[1]->globalCentre().z();
  double zIT3   =  stations[2]->globalCentre().z();

  const State& IT1State = track->closestState(zIT1);
  const State& IT2State = track->closestState(zIT2);
  const State& IT3State = track->closestState(zIT3);
  StateVector* stateVectorIT;

  if(std::string::npos != m_layer.find("IT1")){
    stateVectorIT = new StateVector(IT1State.position(), IT1State.slopes());
  }
  else if(std::string::npos != m_layer.find("IT2")){
    stateVectorIT = new StateVector(IT2State.position(), IT2State.slopes());
  }
  else{
    stateVectorIT = new StateVector(IT3State.position(), IT3State.slopes());
  }
  

  // loop over the sectors
  const DeSTDetector::Sectors& sectorVector = m_STDet->sectors();
  DeSTDetector::Sectors::const_iterator iterS = sectorVector.begin();
  for (; iterS != sectorVector.end(); ++iterS){
    if((*iterS)->elementID().uniqueLayer()==m_layerMap[m_layer]){
      m_extrapolator->propagate((*stateVectorIT),(*iterS)->globalCentre().z());
      return stateVectorIT;
    }
  }
  return 0;
}


 
