#include <string>
#include <bitset>

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

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

#include "ICOMETLog.hxx"
#include "IGeometryId.hxx"
#include "IECALChannelId.hxx"
#include "COMETGeomId.hxx"
#include "COMETGeomIdDef.hxx"
#include "IECALGeomId.hxx"
#include "IECALGeom.hxx"
#include "IECALCrystalMap.hxx"
#include "IGeomInfo.hxx"
#include "IECALCrystalManager.hxx"

#include "EoaGeomInfo.hxx"

//=========================================================================
// IECALCrystalNeighborIndex
//=========================================================================
COMET::IECALCrystalNeighborIndex COMET::IECALCrystalNeighborIndex::Get(const int dx, const int dy)
{
  if(dx<-1 || dx>+1 || dy<-1 || dy>+1) return kNone;
  switch(dy){
  case -1:
    switch(dx){
    case -1: return kBL;
    case  0: return kBC;
    case +1: return kBR;
    }
  case 0:
    switch(dx){
    case -1: return kML;
    case  0: return kNone;
    case +1: return kMR;
    } 
  case +1:
    switch(dx){
    case -1: return kTL;
    case  0: return kTC;
    case +1: return kTR;
    } 
  }
  return kNone;
}

//=========================================================================
// IECALCrystalInfo
//=========================================================================
COMET::IECALCrystalInfo::~IECALCrystalInfo() { }
COMET::IECALCrystalInfo::IECALCrystalInfo () :
  fNodeId(-1),
  fSequenceId(-1),
  fBlockId(-1),
  fCrystalId(-1),
  fPosition(TVector3(0,0,0)),
  f2dPosition(TVector2(0,0)),
  fIdxX(-1),
  fIdxY(-1)
{
}

const COMET::IECALCrystalInfo*
COMET::IECALCrystalInfo::GetNeighbor(const int dx, const int dy) const
{
  if(dx==0 && dy==0) return this;
  return GetNeighbor(COMET::IECALCrystalNeighborIndex::Get(dx, dy));
}

void COMET::IECALCrystalInfo::SetPosition(const TVector3& position)
{
  fPosition = position;
  Set2dPosition(position);
}
  
void COMET::IECALCrystalInfo::Set2dPosition(const TVector3& globalPosition)
{
  TVector3 pos;
  COMET::IGeomInfo::Get().ECAL().MasterToECALLocal(globalPosition, pos);
  f2dPosition = pos.XYvector();
}

//=========================================================================
// IECALCrystalManager
//=========================================================================
COMET::IECALCrystalManager::~IECALCrystalManager()
{
  delete fCrystalInfoMap;
}

COMET::IECALCrystalManager::IECALCrystalManager():
  fNumberOfChannels(0),
  fNumberOfBlocks(0),
  fNumberOfCrystals(0),
  fCrystalInfoMap(NULL)
{
  fNodeIdMap.resize(0);
  fSequenceIdMap.clear();
  fCrystalInfo.clear();
}

// ---------------------------------------------------------------------------------------------------
int COMET::IECALCrystalManager::Init(const INodeIdMap& nodeIdMap)
{
  SetNodeIdMap(nodeIdMap);
  
  int found = 0;

  TGeoNode    *crystalNode;
  TVector3     master; // master position

  for (unsigned int block = 0; block < fNodeIdMap.size(); block++) {
    for (unsigned int crystal = 0; crystal < fNodeIdMap.at(block).size(); crystal++) {
      int nodeId = fNodeIdMap.at(block).at(crystal);
      crystalNode = COMET::IOADatabase::Get().GeomId().GetNode(nodeId);
      if (!crystalNode){
        COMETError("Cannot find node for crystal "<<block<<" "<<crystal<<" with node id = "<<nodeId);
        continue;
      }
      
      // Check the name
      std::string name(crystalNode->GetName());
      if (name.find("Crystal_") == std::string::npos) throw EoaGeomInfo();
      
      // Create crystal info
      COMET::IECALCrystalInfo crystalInfo = COMET::IECALCrystalInfo();
      crystalInfo.SetNodeId(nodeId);
      crystalInfo.SetSequenceId(found);
      crystalInfo.SetGeometryId(COMET::GeomId::ECAL::Module(block, crystal));
      crystalInfo.SetBlockId(block);
      crystalInfo.SetCrystalId(crystal);
      COMET::IOADatabase::Get().GeomId().LocalToMaster(nodeId,0,0,0,master);
      crystalInfo.SetPosition(master);
      fCrystalInfo.push_back(crystalInfo);
      fSequenceIdMap[nodeId] = found;
      
      COMETNamedDebug("IECALCrystalManager",
                      "Created a crystalinfo [node:" << nodeId << "] [ch:" << found << "] @ " << name);
      found++;
      fNumberOfCrystals = crystal;
    }
    fNumberOfBlocks = block;
  }
  fNumberOfChannels = found;
  fNumberOfBlocks   = fNodeIdMap.size();
  if(fNumberOfBlocks>0) fNumberOfCrystals = fNodeIdMap.at(0).size();

  COMETNamedInfo("IECALCrystalManager", "Created channel map (size = " << fSequenceIdMap.size() << ").");

  // Initialize the map with 'Writable'CrystalMap
  delete fCrystalInfoMap;
  fCrystalInfoMap = new COMET::IECALWritableCrystalInfoMap(fCrystalInfo);
  
  return found;
}

bool COMET::IECALCrystalManager::
GetGeomIdFromGlobalPosition(const TVector3& global, IGeometryId& id) const
{
  Int_t sequenceId = GetSequenceIdFromGlobalPosition(global);
  if(0 > sequenceId) return false;
  return GetGeomIdFromSequenceId(sequenceId, id);
}

bool COMET::IECALCrystalManager::
GetGeomIdFromGlobalPosition(const double* global, IGeometryId& id) const
{
  TVector3 pos(global[0], global[1], global[2]);
  return GetGeomIdFromGlobalPosition(pos, id);
}

bool COMET::IECALCrystalManager::
GetGeomIdFromSequenceId(const int sequenceId, IGeometryId& id) const
{
  COMET::IECALCrystalInfo info;
  if(GetCrystalInfo(sequenceId, info)){
    id = info.GetGeometryId();
    return true;
  }
  return false;
}

int COMET::IECALCrystalManager::
GetSequenceIdFromECAL(const int block, const int crystal) const
{
  int sequenceId = fNumberOfCrystals*block + crystal;
  if(sequenceId >= 0 && sequenceId < fNumberOfChannels) return sequenceId;
  else return -1;
}

int COMET::IECALCrystalManager::
GetSequenceIdFromGlobalPosition(const TVector3& global) const
{
  // Project on ECAL plane to avoid to miss nodes
  // because some hits locate around border between the crystal and others.
  // (Due to some rounding hits for IG4HitCalo?)
  TVector3 projPos;
  COMET::IGeomInfo::Get().ECAL().ProjectOnECALPlane(global, projPos);
  // Find node Id
  int crystalNodeId = COMET::IOADatabase::Get().GeomId().FindNodeId(projPos);
  Int_t sequenceId = -1;
  
  if(0 != crystalNodeId){
    COMETNamedDebug("IECALCrystalManager", "found node : [" << crystalNodeId << "]");
    // Get channel id from the map.
    sequenceId = GetSequenceIdFromNodeId(crystalNodeId);

    if(0 > sequenceId){
      // Probably this position is just at a wrapping volume.. Use a tricky way
      TGeoManager* geom = COMET::IOADatabase::Get().GetGeometry();
      geom->PushPath();
      geom->CdNode(crystalNodeId);
      Double_t* nodeCenterPos = geom->GetCurrentMatrix()->GetTranslation();
      // Probably a node of 'crystal' found by
      geom->FindNode(nodeCenterPos[0], nodeCenterPos[1], nodeCenterPos[2]);
      sequenceId = GetSequenceIdFromNodeId(geom->GetCurrentNodeId());
      geom->PopPath();
    }
  }
  if(0 > sequenceId){
    // More tricky way... seek around the hit position
    TVector3 projPosFlucG, projPosFlucL, projPosFlucLBase;
    COMET::IGeomInfo::Get().ECAL().MasterToECALLocal(projPos, projPosFlucLBase);
    for(Double_t fluctuateLength = 1*unit::mm;
        fluctuateLength <= 10*unit::mm; fluctuateLength += unit::mm){
      for(Int_t ifx=-1; ifx<=1; ifx+=2){
        for(Int_t ify=-1; ify<=1; ify+=2){
          if(0 == ifx && 0 == ify) continue;
          projPosFlucL = projPosFlucLBase;
          projPosFlucL[0] += ifx * fluctuateLength;
          projPosFlucL[1] += ify * fluctuateLength;
          COMET::IGeomInfo::Get().ECAL().ECALLocalToMaster(projPosFlucL, projPosFlucG);
          crystalNodeId = COMET::IOADatabase::Get().GeomId().FindNodeId(projPosFlucG);
          if(0 != crystalNodeId){
            sequenceId = GetSequenceIdFromNodeId(crystalNodeId);
            if(0 <= sequenceId) return sequenceId;
          }
        }
      }
    }
  }
  return sequenceId;
}

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

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

int COMET::IECALCrystalManager::
GetSequenceIdFromChannelId(const COMET::IChannelId& chanId) const
{
  // Check if it's an ECAL channel
  if(not chanId.IsECALChannel()) return -1;
  COMET::IECALChannelId ecalCh(chanId);

  // Get block/crystal number
  unsigned int block, crystal;
  ecalCh.GetComponentNumber(block, crystal);
  return GetSequenceIdFromECAL(block, crystal);
}

int COMET::IECALCrystalManager::
GetSequenceIdFromGeomId(const COMET::IGeometryId& geomId) const
{
  int block   = GetBlockIdFromGeomId(geomId);
  int crystal = GetCrystalIdFromGeomId(geomId);
  return GetSequenceIdFromECAL(block, crystal);
}

int COMET::IECALCrystalManager::
GetBlockIdFromSequenceId(const int sequenceId) const
{
  COMET::IECALCrystalInfo crystalInfo;
  if (!GetCrystalInfo(sequenceId, crystalInfo)) return -1;
  int blockId = crystalInfo.GetBlockId();
  if(!(blockId>=0 && blockId<fNumberOfBlocks)) return -1;
  return blockId;
}

int COMET::IECALCrystalManager::
GetCrystalIdFromSequenceId(const int sequenceId) const
{
  COMET::IECALCrystalInfo crystalInfo;
  if (!GetCrystalInfo(sequenceId, crystalInfo)) return -1;
  int crystalId = crystalInfo.GetCrystalId();
  if(!(crystalId>=0 && crystalId<fNumberOfCrystals)) return -1;
  return crystalId;
}

TGeoNode* COMET::IECALCrystalManager::
GetNode(const int block, const int crystal) const
{
  COMET::IECALCrystalInfo crystalInfo;
  int sequenceId = GetSequenceIdFromECAL(block, crystal);
  if (!GetCrystalInfo(sequenceId, crystalInfo)) return NULL;
  int nodeId =  crystalInfo.GetNodeId();

  TGeoNode* node = COMET::IOADatabase::Get().GeomId().GetNode(nodeId);
  if (!node){
    COMETError("Cannot find node for crystal with node id = "<<nodeId);
    return 0;
  }
  std::string name(node->GetName());
  if (name.find("Crystal") == std::string::npos) return NULL;
  return node;
}

TVector3 COMET::IECALCrystalManager::GetCrystalPosition(const int sequenceId) const
{
  COMET::IECALCrystalInfo crystalInfo;
  if (!GetCrystalInfo(sequenceId, crystalInfo)) return TVector3(1e9, 1e9, 1e9);
  return crystalInfo.GetPosition();
}

TVector3 COMET::IECALCrystalManager::GetCrystalPosition(const IGeometryId& id) const
{
  int sequenceId = GetSequenceIdFromGeomId(id);
  return GetCrystalPosition(sequenceId);
}

TVector3 COMET::IECALCrystalManager::
GetCrystalPosition(const int block, const int crystal) const
{
  int sequenceId = GetSequenceIdFromECAL(block, crystal);
  return GetCrystalPosition(sequenceId);
}

int COMET::IECALCrystalManager::
GetBlockIdFromGeomId(const IGeometryId& id) const
{
  int block = COMET::GeomId::ECAL::GetBlockId(id);
  if (block>=0 && block<fNumberOfBlocks) return block;
  else return -1;
}

int COMET::IECALCrystalManager::
GetCrystalIdFromGeomId(const IGeometryId& id) const
{
  int crystal = COMET::GeomId::ECAL::GetCrystalId(id);
  if (crystal>=0 && crystal<fNumberOfCrystals) return crystal;
  else return -1;
}


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

  fNodeIdMap.resize(0);

  // Loop for blocks
  for (unsigned int ii=0; ii<nodeMap.size(); ii++) {
    std::vector<int> tmp;
    tmp.resize(0);
    // Loop for crystals in a block
    for (unsigned int jj=0; jj<nodeMap.at(ii).size(); jj++) {
      if (nodeMap.at(ii).at(jj)<0) continue;
      tmp.push_back(nodeMap.at(ii).at(jj));
      ++fNumberOfChannels;
    }
    if(tmp.size() > 0) fNodeIdMap.push_back(tmp);
  }
  if (!fNumberOfChannels) return false;

  fNumberOfBlocks = fNodeIdMap.size();
  if(fNumberOfBlocks>0) fNumberOfCrystals = fNodeIdMap.at(0).size();
  
  return true;
}


bool COMET::IECALCrystalManager::
GetCrystalInfo (const int sequenceId, COMET::IECALCrystalInfo& info) const
{
  if ((unsigned int)sequenceId < fCrystalInfo.size()) {
    info = fCrystalInfo.at(sequenceId);
    return true;
  }
  return false;
}


const COMET::IECALCrystalInfoMap& COMET::IECALCrystalManager::GetCrystalInfoMap() const
{
  return *fCrystalInfoMap;
}