// Include files 
#include "STHitIntegrator.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 "GaudiAlg/GaudiTupleAlg.h"

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

// 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( STHitIntegrator )

//=============================================================================
// Standard constructor, initializes variables
//=============================================================================
STHitIntegrator::STHitIntegrator(const std::string& name,
                           ISvcLocator* pSvcLocator ) : GaudiTupleAlg(name, pSvcLocator)
{
  declareProperty("DetType",m_detType,"TT");
  declareProperty("Layer",m_layer,"TTaU");
  declareProperty("MaxX",     m_maxX=      600.0);
  declareProperty("MaxY",     m_maxY=      600.0);

  declareProperty("IntSteps", m_Ntestint=   1e+8);
  
  

  // 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
//=============================================================================
STHitIntegrator::~STHitIntegrator() {}; 

StatusCode STHitIntegrator::initialize()
{
  // Mandatory initialization of GaudiAlgorithm
  

  Tuples::Tuple m_tuple           = GaudiTupleAlg::nTuple(m_layer,"TrackInfo",CLID_ColumnWiseTuple );
  
  m_STDet           = getDet<DeSTDetector>(DeSTDetLocation::location(m_detType));

  // Get all sectors
  const DeSTDetector::Sectors& sectors = m_STDet->sectors();

  // Check if they are in the layer you are interested

  std::vector<DeSTSector*> sel_sectors;

  for(unsigned int i=0; i<sectors.size(); i++){
    
    LHCb::STChannelID tempID = sectors[i]->stripToChan(sectors[i]->nStrip()/2);
    debug() << "Sector " << tempID.uniqueSector() << " is in HM "<< sectors[i]->prodID() << endmsg;
    
    if(((tempID.isTT() && m_detType=="TT") ||
        (tempID.isIT() && m_detType=="IT")) &&
       tempID.uniqueLayer()==m_layerMap[m_layer]){
      debug() << "Sector " << tempID.uniqueSector() << " at " << m_layer << " selected!" << endmsg; 
      sel_sectors.push_back(sectors[i]);
    } 
  }

  srand(time(NULL));
  

  for(int i=0; i<m_Ntestint; i++){
    double x = (double)rand()*2.0*m_maxX/RAND_MAX - m_maxX;
    double y = (double)rand()*2.0*m_maxY/RAND_MAX - m_maxY;
    debug() << "Point (x,y,r) = (" <<x<<","<<y<<","<<sqrt(x*x+y*y)<<")"<<endmsg;
    

    for(unsigned int j=0; j<sel_sectors.size(); j++){
      LHCb::STChannelID tempID = sel_sectors[j]->stripToChan(sel_sectors[j]->nStrip()/2);
      std::string sec = boost::lexical_cast<std::string>(tempID.uniqueSector());
      
      if(sel_sectors[j]->globalInActive(Gaudi::XYZPoint(x,y,0.0))){
        
        m_tuple->column("x",x*0.1);
        m_tuple->column("y",y*0.1);
        m_tuple->column("Sec",tempID.uniqueSector());
        StatusCode status = m_tuple->write();
        debug() << "Hit matches sensor! " << sec <<endmsg;
        if(status.isFailure()){
          return Error("Cannot fill ntuple");
        }
        
        
      }
    }
    if(i%10000==0){
      info() << "Test event "<<i<<" processed!"<<endmsg;
    }
    
  }
  
  
  return StatusCode::SUCCESS;
}


