#include <string>
#include <set>
#include <algorithm>
#include <numeric>

#include <TGeoManager.h>
#include <TGeoNode.h>
#include <TMath.h>

#include "HEPUnits.hxx"
#include "HEPConstants.hxx"
#include "IOADatabase.hxx"
#include "IGeomIdManager.hxx"

#include "ICOMETLog.hxx"
#include "IStrawTrkChannelId.hxx"
#include "IGeometryId.hxx"
#include "COMETGeomId.hxx"
#include "COMETGeomIdDef.hxx"
#include "IStrawTrkGeomId.hxx"
#include "IStrawTrkWireManager.hxx"

#include "IGeomInfo.hxx"
#include "IDetectorSolenoidGeom.hxx"

#include "EoaGeomInfo.hxx"

COMET::IStrawTrkWireInfo::~IStrawTrkWireInfo() { }

COMET::IStrawTrkWireInfo::IStrawTrkWireInfo () :
  fNodeId(-1),
  fSequenceId(-1),
  fStationId(-1),
  fManifoldId(-1),
  fStrawId(-1),
  fPosition(TVector3(0,0,0)),
  fEnd0Pos(TVector3(0,0,0)),
  fEnd1Pos(TVector3(0,0,0)),
  fDirectionIsY(false)
{
}

COMET::IStrawTrkWireManager::~IStrawTrkWireManager() { }

COMET::IStrawTrkWireManager::IStrawTrkWireManager()
  : fNumberOfChannels(0),
    fNumberOfStations(0),
    fNumberOfManifolds(0),
    fNumberOfStraws(0)
{
  fNodeIdMap.resize(0);
  fSequenceIdMap.clear();
  fStrawTrkInfo.clear();
}

// ---------------------------------------------------------------------------------------------------
int COMET::IStrawTrkWireManager::Init(const INodeIdMap& nodeIdMap)
{
  SetNodeIdMap(nodeIdMap);

  int found = 0;
  fStationPositionZInDS       .clear();
  fGlobalLayerPositionZInDS   .clear();
  fGlobalLayerDirectionIsY    .clear();  
  fGlobalSubLayerPositionZInDS.clear();
  fGlobalSubLayerDirectionIsY .clear();

  TGeoNode    *strawTrkNode;
  IGeometryId  geomId;
  TVector3 master(0,0,0); // master position
  TVector3 end0master(0,0,0);
  TVector3 end1master(0,0,0);
  TVector3 wireDir, wireDirDs;
  TVector3 posDS;
  std::set<Double_t> manifoldZInDS[2]; // [manifoldId]

  bool wireDirIsY;
  
  COMET::IDetectorSolenoidGeom& dsGeom = COMET::IGeomInfo::DetectorSolenoid();
  Double_t strawRadius = 0.5 * COMET::IGeomInfo::StrawTrk().GetStrawDiameter();
  
  // Z axis in DS coord for defining wire direction
  TVector3 zAxisDs(0,0,+1), mAxisDs;

  for (unsigned int station = 0; station < fNodeIdMap.size(); station++) {
    // Check which manifold is located in front of each other
    manifoldZInDS[0].clear();
    manifoldZInDS[1].clear();
    
    for (unsigned int manifold = 0; manifold < fNodeIdMap.at(station).size(); manifold++) {
      for (unsigned int straw = 0; straw < fNodeIdMap.at(station).at(manifold).size(); straw++) {
        int nodeId = fNodeIdMap.at(station).at(manifold).at(straw);
        strawTrkNode = COMET::IOADatabase::Get().GeomId().GetNode(nodeId);
        if (!strawTrkNode){
          COMETError("Cannot find node for straw tube "<<station<<" "<<manifold<<" "<<straw<<" with node id = "<<nodeId);
          continue;
        }
        
        // Check the name
        // std::string name(strawTrkNode->GetName());
        // if (name.find("StrawGas") != std::string::npos) {
        TVector3 nodeSize = COMET::IOADatabase::Get().GeomId().NodeSize(strawTrkNode, COMET::IGeomIdManager::kTube);
  
        // Create wire info
        COMET::IStrawTrkWireInfo wireInfo;
        wireInfo.SetNodeId(nodeId);
        wireInfo.SetSequenceId(found);
        wireInfo.SetStationId(station);
        wireInfo.SetManifoldId(manifold);
        wireInfo.SetStrawId(straw);
        wireInfo.SetLength(2. * nodeSize.Z());
        COMET::IOADatabase::Get().GeomId().LocalToMaster(nodeId,0,0,0,             master);
        COMET::IOADatabase::Get().GeomId().LocalToMaster(nodeId,0,0,-nodeSize.Z(), end0master);
        COMET::IOADatabase::Get().GeomId().LocalToMaster(nodeId,0,0,+nodeSize.Z(), end1master);

        // Wire direction
        wireDir = end1master - end0master;
        dsGeom.GetDetVectorInDSCoordinate(wireDir, wireDirDs);
        // if wire direction is Y
        if(TMath::Abs(wireDirDs.X()) < TMath::Abs(wireDirDs.Y())) wireDirIsY = true;
        else                                                      wireDirIsY = false;
  
        // Check the wire end positions in the detector solenoid coordinates
        // Measurement axis near the wire is defined as (wire direction) x (momentum direction)
        // This is based on GENFIT's state vector coordinates: w(plane dir) = u(meas. dir) x v(wire dir)
        // Since the momentum direction is the same as the beam axis,
        // here the z-axis in DS coord. is used as the momentum direction instead.
        // In addition, the measurment axis is supposed to face +X(Y) direction in DS coord.
        mAxisDs = wireDirDs.Cross(zAxisDs);
        if( (    wireDirIsY && 0 > mAxisDs.X() ) ||
            (not wireDirIsY && 0 > mAxisDs.Y() )   ) std::swap(end0master, end1master);
  
        wireInfo.SetPosition(master);
        wireInfo.SetEnd0Pos(end0master);
        wireInfo.SetEnd1Pos(end1master);
        wireInfo.SetDirectionIsY(wireDirIsY);
  
        // Add the info to the map
        fStrawTrkInfo.push_back(wireInfo);
        fSequenceIdMap[nodeId] = found;

        wireDir = (end1master - end0master).Unit();  
        COMETNamedDebug("IStrawTrkWireManager",
                        "Created a wireinfo [node:" << nodeId << "] [ch:" << found << "] @ " << strawTrkNode->GetName()
                        << ", end1&0 = (" << std::setprecision(3)
                        << end1master.X() << "," << end1master.Y() << "," << end1master.Z() << ") & ("
                        << end0master.X() << "," << end0master.Y() << "," << end0master.Z() << "), dir = ("
                        << std::setprecision(1)
                        << wireDir.X()    << "," << wireDir.Y()    << "," << wireDir.Z()    << ")");
        found++;

        // ---------------------------------------------------------------
        // Manifold (layer) position in the detector solenoid coordinates
        // ---------------------------------------------------------------  
        dsGeom.GetDetPositionInDSCoordinate(master, posDS);
        // Check this z position is already stored
        bool newZpos = true;
        for(std::set<Double_t>::iterator
              zit = manifoldZInDS[manifold].begin(); zit != manifoldZInDS[manifold].end(); zit++){
          if(strawRadius > std::abs(*zit - posDS.Z())){
            newZpos = false;
            break;
          }
        }
        if(newZpos) manifoldZInDS[manifold].insert(posDS.Z());
      }
      fNumberOfManifolds = manifold;
    }    
    fNumberOfStations = station;

    // Check the number of sub layers
    if(manifoldZInDS[0].size() != manifoldZInDS[1].size()){
      COMETNamedError("IStrawTrkWireManager", "Init() : "
                      << "The same number of sublayers on X/Y are not different!");
      throw COMET::EoaGeomInfo();
    }
    
    // Average center position Z in DS of each manifold (layer)
    Double_t avePosZ[2] =
      { std::accumulate(manifoldZInDS[0].begin(), manifoldZInDS[0].end(), 0.0) / manifoldZInDS[0].size(),
        std::accumulate(manifoldZInDS[1].begin(), manifoldZInDS[1].end(), 0.0) / manifoldZInDS[1].size() };
    
    // Determin the manifold (layer) in more front side.
    if(0 == station){
      if( avePosZ[0] < avePosZ[1] ) fManifoldIdOfFrontLayer = 0;
      else                          fManifoldIdOfFrontLayer = 1;
      fNumberOfSubLayers = (int)manifoldZInDS[0].size();
    }
    // Check the number of sub layers
    if((int)manifoldZInDS[0].size() != fNumberOfSubLayers){
      COMETNamedError("IStrawTrkWireManager", "Init() : "
                      << "The number of sublayers is not the same as the others!");
      throw COMET::EoaGeomInfo();
    }
    
    // Center of global-layer position Z in DS
    fGlobalLayerPositionZInDS.push_back( std::min(avePosZ[0], avePosZ[1]) );
    fGlobalLayerPositionZInDS.push_back( std::max(avePosZ[0], avePosZ[1]) );
    // Center of station position Z in DS
    fStationPositionZInDS.push_back( 0.5 * (avePosZ[0]+avePosZ[1]) );
    
    // Center of global-sublayer position Z in DS
    std::copy(manifoldZInDS[fManifoldIdOfFrontLayer].begin(),
              manifoldZInDS[fManifoldIdOfFrontLayer].end(),
              std::back_inserter(fGlobalSubLayerPositionZInDS));
    std::copy(manifoldZInDS[1-fManifoldIdOfFrontLayer].begin(),
              manifoldZInDS[1-fManifoldIdOfFrontLayer].end(),
              std::back_inserter(fGlobalSubLayerPositionZInDS));
  }
  fNumberOfChannels = found;
  fNumberOfStations = fNodeIdMap.size();
  if(fNumberOfStations>0)  fNumberOfManifolds = fNodeIdMap.at(0).size();
  if(fNumberOfManifolds>0) fNumberOfStraws = fNodeIdMap.at(0).at(0).size();

  // -------------------------------------------------
  // Assign global layer and sublayer Ids
  // -------------------------------------------------
  fGlobalLayerDirectionIsY       .resize(fGlobalLayerPositionZInDS.size());
  fGlobalLayerId2StationId       .resize(fGlobalLayerPositionZInDS.size());
  fGlobalSubLayerDirectionIsY    .resize(fGlobalSubLayerPositionZInDS.size());
  fGlobalSubLayerId2GlobalLayerId.resize(fGlobalSubLayerPositionZInDS.size());
  for(IWireInfoContainer::iterator
        wit = fStrawTrkInfo.begin(); wit != fStrawTrkInfo.end(); wit++){
    dsGeom.GetDetPositionInDSCoordinate(wit->GetPosition(), posDS);
    // Global layer Id
    for(unsigned zi = 0; zi < fGlobalLayerPositionZInDS.size(); zi++){
      if(fNumberOfSubLayers * strawRadius > std::abs(fGlobalLayerPositionZInDS[zi] - posDS.Z())){
        wit->SetGlobalLayerId(zi);
        // Layer's direction
        fGlobalLayerDirectionIsY[zi] = wit->GetDirectionIsY();
        // Look up table
        fGlobalLayerId2StationId[zi] = wit->GetStationId();
        break;
      }
    }
    // Global sub layer Id
    for(unsigned zi = 0; zi < fGlobalSubLayerPositionZInDS.size(); zi++){
      if(strawRadius > std::abs(fGlobalSubLayerPositionZInDS[zi] - posDS.Z())){
        wit->SetGlobalSubLayerId(zi);
        // Sublayer's direction
        fGlobalSubLayerDirectionIsY[zi] = wit->GetDirectionIsY();
        // Look up table
        fGlobalSubLayerId2GlobalLayerId[zi] = wit->GetGlobalLayerId();
        break;
      }
    }

    COMETNamedVerbose("IStrawTrkWireManager", "Init() : "
                      << "Straw SeqId=" << wit->GetSequenceId() << " : "
                      << "Global Layer=" << wit->GetGlobalLayerId() << " / "
                      << "Global Sublayer=" << wit->GetGlobalSubLayerId()
                      << " at Z in DS = " << posDS.Z());
  }

  if(COMET::ICOMETLog::InfoLevel <=
     COMET::ICOMETLog::GetLogLevel("IStrawTrkWireManager")){

    COMETNamedInfo("IStrawTrkWireManager",
                   "#Stations = " << GetNumberOfStations()
                   << " #Layers = " << GetNumberOfGlobalLayers()
                   << " #SubLayers " << GetNumberOfGlobalSubLayers());
    COMETNamedInfo("IStrawTrkWireManager",
                   "SubLayer -> Layer-> Station : Lookup table and Z-position in DS-coordinates.");
    for(int i=0; i<GetNumberOfGlobalSubLayers(); i++){
      COMETNamedInfo("IStrawTrkWireManager",
                     Form("%2d (%+7.1f mm) -> %2d (%+7.1f mm) -> %2d", i,
                          GetGlobalSubLayerPositionZInDSFromGlobalSubLayerId(i),
                          GetGlobalLayerIdFromGlobalSubLayerId(i),
                          GetGlobalLayerPositionZInDSFromGlobalLayerId(GetGlobalLayerIdFromGlobalSubLayerId(i)),
                          GetStationIdFromGlobalSubLayerId(i)));
    }
  }
  COMETNamedInfo("IStrawTrkWireManager", "Created channel map (size = " << fSequenceIdMap.size() << ").");  
  return found;
}


int COMET::IStrawTrkWireManager::
GetSequenceIdFromStrawTrk(const int& station, const int& manifold, const int& straw) const
{
  int sequenceId = (fNumberOfManifolds*fNumberOfStraws)*station + (fNumberOfStraws)*manifold + straw;
  if(sequenceId >= 0 && sequenceId < fNumberOfChannels) return sequenceId;
  else return -1;
}


int COMET::IStrawTrkWireManager::
GetSequenceIdFromGlobalPosition(const TVector3& global) const
{
  Int_t strawTrkNodeId = COMET::IOADatabase::Get().GeomId().FindNodeId(global);
  Int_t sequenceId = -1;
  
  if(0 != strawTrkNodeId){
    COMETNamedDebug("IStrawTrkWireManager",
                    "found node : [" << strawTrkNodeId << "] ");
    // Get channel id from the map.
    sequenceId = GetSequenceIdFromNodeId(strawTrkNodeId);

    if(sequenceId<0){
      // Probably this position is just at tube or wire volume.. Use a tricky way
      TGeoManager* geom = COMET::IOADatabase::Get().GetGeometry();
      geom->PushPath();
      geom->CdNode(strawTrkNodeId);
      Double_t* nodeCenterPos = geom->GetCurrentMatrix()->GetTranslation();
      // Probably a node of 'wire' is found by
      geom->FindNode(nodeCenterPos[0], nodeCenterPos[1], nodeCenterPos[2]);
      // Move 'wire' -> 'gas'
      geom->CdUp();
      sequenceId = GetSequenceIdFromNodeId(geom->GetCurrentNodeId());
      geom->PopPath();
    }
  }
  return sequenceId;
}

int COMET::IStrawTrkWireManager::
GetSequenceIdFromGlobalPosition(const double* global) const
{
  TVector3 pos(global[0], global[1], global[2]);
  return GetSequenceIdFromGlobalPosition(pos);
}


int COMET::IStrawTrkWireManager::
GetSequenceIdFromNodeId(const int& nodeId) const
{
  if(fSequenceIdMap.find(nodeId) == fSequenceIdMap.end()) {
    return -1;
  }
  else {
    return fSequenceIdMap.find(nodeId)->second;
  }
}


int COMET::IStrawTrkWireManager::
GetSequenceIdFromChannelId(const COMET::IChannelId& chanId) const
{
  // Check if it's a StrawTrk channel
  if(not chanId.IsStrawTrkChannel()) return -1;
  COMET::IStrawTrkChannelId strawCh(chanId);

  // Get station/manifold/straw Ids
  unsigned int station, manifold, straw;
  strawCh.GetModuleId(station, manifold, straw);
  return GetSequenceIdFromStrawTrk(station, manifold, straw);
}


int COMET::IStrawTrkWireManager::
GetSequenceIdFromGeomId(const COMET::IGeometryId& geomId) const
{
  int station  = GetStationIdFromGeomId(geomId);
  int manifold = GetManifoldIdFromGeomId(geomId);
  int straw    = GetStrawIdFromGeomId(geomId);
  if(station<0||manifold<0||straw<0) return -1;
  return GetSequenceIdFromStrawTrk(station, manifold, straw);
}


int COMET::IStrawTrkWireManager::
GetStationIdFromSequenceId(const int& sequenceId) const
{
  if(!IsValid(sequenceId)) return -1;
  int stationId = GetStrawTrkInfo(sequenceId).GetStationId();
  if(!(stationId>=0 && stationId<fNumberOfStations)) return -1;
  return stationId;
}


int COMET::IStrawTrkWireManager::
GetManifoldIdFromSequenceId(const int& sequenceId) const
{
  if(!IsValid(sequenceId)) return -1;
  int manifoldId = GetStrawTrkInfo(sequenceId).GetManifoldId();
  if(!(manifoldId>=0 && manifoldId<fNumberOfManifolds)) return -1;
  return manifoldId;
}


int COMET::IStrawTrkWireManager::
GetStrawIdFromSequenceId(const int& sequenceId) const
{
  if(!IsValid(sequenceId)) return -1;
  int strawId = GetStrawTrkInfo(sequenceId).GetStrawId();
  if(!(strawId>=0 && strawId<fNumberOfStraws)) return -1;
  return strawId;
}

int COMET::IStrawTrkWireManager::
GetGlobalLayerIdFromSequenceId(const int& sequenceId) const
{
  if(!IsValid(sequenceId)) return -1;
  const IStrawTrkWireInfo& info = GetStrawTrkInfo(sequenceId);
  return info.GetGlobalLayerId();
}

int COMET::IStrawTrkWireManager::
GetGlobalSubLayerIdFromSequenceId(const int& sequenceId) const
{
  if(!IsValid(sequenceId)) return -1;
  const IStrawTrkWireInfo& info = GetStrawTrkInfo(sequenceId);
  return info.GetGlobalSubLayerId();
}

int COMET::IStrawTrkWireManager::
GetStationIdFromGlobalLayerId(const int glayerId) const
{
  return fGlobalLayerId2StationId[glayerId];
}
int COMET::IStrawTrkWireManager::
GetStationIdFromGlobalSubLayerId(const int gsublayerId) const
{
  return GetStationIdFromGlobalLayerId(GetGlobalLayerIdFromGlobalSubLayerId(gsublayerId));
}
int COMET::IStrawTrkWireManager::
GetGlobalLayerIdFromGlobalSubLayerId(const int gsublayerId) const
{
  return fGlobalSubLayerId2GlobalLayerId[gsublayerId];
}



TVector3 COMET::IStrawTrkWireManager::GetWirePosition(const IGeometryId& id) const
{
  int sequenceId = GetSequenceIdFromGeomId(id);
  if(!IsValid(sequenceId)) return TVector3(1e9, 1e9, 1e9);
  return GetStrawTrkInfo(sequenceId).GetPosition();
}


TVector3 COMET::IStrawTrkWireManager::
GetWirePosition(const int& station, const int& manifold, const int& straw) const
{
  int sequenceId = GetSequenceIdFromStrawTrk(station, manifold, straw);
  if(!IsValid(sequenceId)) return TVector3(1e9, 1e9, 1e9);
  return GetStrawTrkInfo(sequenceId).GetPosition();
}


TVector3 COMET::IStrawTrkWireManager::
GetWireEnd0(const int& station, const int& manifold, const int& straw) const
{
  int sequenceId = GetSequenceIdFromStrawTrk(station, manifold, straw);
  if(!IsValid(sequenceId)) return TVector3(1e9, 1e9, 1e9);
  return GetStrawTrkInfo(sequenceId).GetEnd0Pos();
}

TVector3 COMET::IStrawTrkWireManager::
GetWireEnd1(const int& station, const int& manifold, const int& straw) const
{
  int sequenceId = GetSequenceIdFromStrawTrk(station, manifold, straw);
  if(!IsValid(sequenceId)) return TVector3(1e9, 1e9, 1e9);
  return GetStrawTrkInfo(sequenceId).GetEnd1Pos();
}

bool COMET::IStrawTrkWireManager::
GetWireDirectionIsY(const int& station, const int& manifold, const int& straw) const
{
  int sequenceId = GetSequenceIdFromStrawTrk(station, manifold, straw);
  if(!IsValid(sequenceId)){
    COMETError("IStrawTrkWireManager::GetWireDirectionY() cannot find valid sequence ID.");
    return false;
  }
  return GetStrawTrkInfo(sequenceId).GetDirectionIsY();
}

Double_t COMET::IStrawTrkWireManager::
GetGlobalLayerPositionZInDS(const int& station, const int& manifold) const
{
  int sequenceId = GetSequenceIdFromStrawTrk(station, manifold, /*dummy*/0);
  return GetGlobalLayerPositionZInDS(sequenceId);
}

Double_t COMET::IStrawTrkWireManager::
GetGlobalSubLayerPositionZInDS(const int& station, const int& manifold, const int& straw) const
{
  int sequenceId = GetSequenceIdFromStrawTrk(station, manifold, straw);
  return GetGlobalSubLayerPositionZInDS(sequenceId);
}

TVector3 COMET::IStrawTrkWireManager::GetWirePosition(int sequenceId) const
{
  if(!IsValid(sequenceId)) return TVector3(1e9, 1e9, 1e9);
  return GetStrawTrkInfo(sequenceId).GetPosition();
}

TVector3 COMET::IStrawTrkWireManager::GetWireEnd0(int sequenceId) const
{
  if(!IsValid(sequenceId)) return TVector3(1e9, 1e9, 1e9);
  return GetStrawTrkInfo(sequenceId).GetEnd0Pos();
}

TVector3 COMET::IStrawTrkWireManager::GetWireEnd1(int sequenceId) const
{
  if(!IsValid(sequenceId)) return TVector3(1e9, 1e9, 1e9);
  return GetStrawTrkInfo(sequenceId).GetEnd1Pos();
}

bool COMET::IStrawTrkWireManager::GetWireDirectionIsY(const int sequenceId) const 
{
  if(!IsValid(sequenceId)){
    COMETError("IStrawTrkWireManager::GetWireDirectionY() cannot find valid sequence ID.");
    return false;
  }
  return GetStrawTrkInfo(sequenceId).GetDirectionIsY();
}

Double_t COMET::IStrawTrkWireManager::GetStationPositionZInDS(const int sequenceId) const 
{
  if(!IsValid(sequenceId)){
    COMETError("IStrawTrkWireManager::GetGlobalLayerPositionZInDS() cannot find valid sequence ID.");
    return -1e30;
  }
  Int_t stationId = GetStationIdFromSequenceId(sequenceId);
  return fStationPositionZInDS[stationId];
}

Double_t COMET::IStrawTrkWireManager::GetGlobalLayerPositionZInDS(const int sequenceId) const 
{
  if(!IsValid(sequenceId)){
    COMETError("IStrawTrkWireManager::GetGlobalLayerPositionZInDS() cannot find valid sequence ID.");
    return -1e30;
  }
  Int_t globalLayerId = GetGlobalLayerIdFromSequenceId(sequenceId);
  return fGlobalLayerPositionZInDS[globalLayerId];
}

Double_t COMET::IStrawTrkWireManager::GetGlobalSubLayerPositionZInDS(const int sequenceId) const 
{
  if(!IsValid(sequenceId)){
    COMETError("IStrawTrkWireManager::GetGlobalSubLayerPositionZInDS() cannot find valid sequence ID.");
    return -1e30;
  }
  Int_t globalSubLayerId = GetGlobalSubLayerIdFromSequenceId(sequenceId);
  return fGlobalSubLayerPositionZInDS[globalSubLayerId];
}

Double_t COMET::IStrawTrkWireManager::
GetStationPositionZInDSFromStationId(const int stationId) const 
{
  if(0 > stationId || GetNumberOfStations() <= stationId) return -1e30;
  return fStationPositionZInDS[stationId];
}

Double_t COMET::IStrawTrkWireManager::
GetGlobalLayerPositionZInDSFromGlobalLayerId(const int glayerId) const 
{
  if(0 > glayerId || GetNumberOfGlobalLayers() <= glayerId) return -1e30;
  return fGlobalLayerPositionZInDS[glayerId];
}

bool COMET::IStrawTrkWireManager::
GetGlobalLayerDirectionIsYFromGlobalLayerId(const int glayerId) const 
{
  if(0 > glayerId || GetNumberOfGlobalLayers() <= glayerId) return false;
  return fGlobalLayerDirectionIsY[glayerId];
}

Double_t COMET::IStrawTrkWireManager::
GetGlobalSubLayerPositionZInDSFromGlobalSubLayerId(const int gsublayerId) const 
{
  if(0 > gsublayerId || GetNumberOfGlobalSubLayers() <= gsublayerId) return -1e30;
  return fGlobalSubLayerPositionZInDS[gsublayerId];
}

bool COMET::IStrawTrkWireManager::
GetGlobalSubLayerDirectionIsYFromGlobalSubLayerId(const int gsublayerId) const 
{
  if(0 > gsublayerId || GetNumberOfGlobalSubLayers() <= gsublayerId) return false;
  return fGlobalSubLayerDirectionIsY[gsublayerId];
}

double COMET::IStrawTrkWireManager::
GetDistanceFromWire(const TVector3& global) const
{
  TVector3 local;
  int strawTrkNodeId = COMET::IOADatabase::Get().GeomId().FindNodeId(global);
  if (not COMET::IOADatabase::Get().GeomId().MasterToLocal(strawTrkNodeId,global,local)) {
    return -1;
  }
  double distance = local.Perp();
  if(0<distance) return distance;
  else return -1.;
}

double COMET::IStrawTrkWireManager::
GetDistanceFromWire(int sequenceId, const TVector3& global) const
{
  if(!IsValid(sequenceId)) return -1.;
  TVector3 local(0,0,0);
  COMET::IOADatabase::Get().GeomId().MasterToLocal(GetStrawTrkInfo(sequenceId).GetNodeId(), global, local);
  double distance = local.Perp();
  return distance;
}

TVector3 COMET::IStrawTrkWireManager::
GetLocalPosition(int sequenceId, const TVector3& global) const
{
  if(!IsValid(sequenceId)) return TVector3(1e10,1e10,1e10);
  TVector3 local(0,0,0);
  COMET::IOADatabase::Get().GeomId().MasterToLocal(GetStrawTrkInfo(sequenceId).GetNodeId(), global, local);
  return TVector3(local[0],local[1],local[2]);
}

TVector3 COMET::IStrawTrkWireManager::
GetLocalVector(int sequenceId, const TVector3& global) const
{
  if(!IsValid(sequenceId)) return TVector3(1e10,1e10,1e10);
  TVector3 local(0,0,0);
  COMET::IOADatabase::Get().GeomId().MasterToLocalVect(GetStrawTrkInfo(sequenceId).GetNodeId(), global, local);
  return TVector3(local[0],local[1],local[2]);
}

int COMET::IStrawTrkWireManager::
GetStationIdFromGeomId(const IGeometryId& id) const
{
  int station = COMET::GeomId::StrawTrk::GetStationId(id);
  if (station>=0 && station<fNumberOfStations) return station;
  else return -1;
}

int COMET::IStrawTrkWireManager::
GetManifoldIdFromGeomId(const IGeometryId& id) const
{
  int manifold = COMET::GeomId::StrawTrk::GetManifoldId(id);
  if (manifold>=0 && manifold<fNumberOfManifolds) return manifold;
  else return -1;
}

int COMET::IStrawTrkWireManager::
GetStrawIdFromGeomId(const IGeometryId& id) const
{
  int straw = COMET::GeomId::StrawTrk::GetStrawId(id);
  if (straw>=0 && straw<fNumberOfStraws) return straw;
  else return -1;
}

int COMET::IStrawTrkWireManager::
GetGlobalLayerIdFromGeomId(const IGeometryId& id) const
{
  int sequenceId = GetSequenceIdFromGeomId(id);
  return GetGlobalLayerIdFromSequenceId(sequenceId);
}

int COMET::IStrawTrkWireManager::
GetGlobalSubLayerIdFromGeomId(const IGeometryId& id) const
{
  int sequenceId = GetSequenceIdFromGeomId(id);
  return GetGlobalSubLayerIdFromSequenceId(sequenceId);
}

bool COMET::IStrawTrkWireManager::
SetNodeIdMap(const INodeIdMap& nodeMap)
{
  fNumberOfChannels = 0;

  fNodeIdMap.clear();

  for (unsigned int ii=0; ii<nodeMap.size(); ii++) {
      
    std::vector < std::vector<int > >tmp1;
    tmp1.resize(0);
    for (unsigned int jj=0; jj<nodeMap.at(ii).size(); jj++) {
      std::vector < int > tmp2;
      tmp2.resize(0);
      for (unsigned int kk=0; kk<nodeMap.at(ii).at(jj).size(); kk++) {
        if (nodeMap.at(ii).at(jj).at(kk)<0) continue;
        tmp2.push_back(nodeMap.at(ii).at(jj).at(kk));
        ++fNumberOfChannels;
      }
      if (tmp2.size()>0) tmp1.push_back(tmp2);
    }
    fNodeIdMap.push_back(tmp1);
  }
  if (!fNumberOfChannels) return false;

  fNumberOfStations = fNodeIdMap.size();
  if(fNumberOfStations>0)  fNumberOfManifolds = fNodeIdMap.at(0).size();
  if(fNumberOfManifolds>0) fNumberOfStraws    = fNodeIdMap.at(0).at(0).size();

  return true;
}

bool COMET::IStrawTrkWireManager::IsValid(int sequenceId) const
{
  return (sequenceId>=0 && (unsigned int)sequenceId<fStrawTrkInfo.size());
}