#include <string>

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

#include "HEPUnits.hxx"
#include "HEPConstants.hxx"
#include "IOADatabase.hxx"
#include "IGeomIdManager.hxx"
#include "ICDCWireManager.hxx"
#include "ICOMETLog.hxx"
#include "IGeomInfo.hxx"
#include "IGeometryId.hxx"
#include "COMETGeomId.hxx"
#include "COMETGeomIdDef.hxx"
#include "ICDCGeomId.hxx"
#include "EoaGeomInfo.hxx"

using namespace COMET;

ICDCWireManager::ICDCWireManager():
fNumberOfLayers(0),
fNumberOfWires(0),
fWireToNodeId(0),
fInnerMostLayerNodeId(-1),
fIsSingleCyDet(false)
{
  fWireInfoMap.clear();
}

ICDCWireManager::~ICDCWireManager() { }

int ICDCWireManager::Init(const std::vector< std::vector< int > >& wiremap, int detSolNodeId, int innerMostLayerNodeId) {
  // Set the wire map
  SetWireMap(wiremap);
  // Get the board mapping
  std::map<int, std::map<int, int> > boardMap;
  GetBoardMap("$OAGEOMINFOROOT/parameters/chanmap_20180416.root", boardMap);

  // Get the geometry
  TGeoNode* cdcNode;
  /// get the node of DetectorSolenoid
  if (detSolNodeId<0) {
    fIsSingleCyDet = true;
  }
  fInnerMostLayerNodeId = innerMostLayerNodeId;

  TVector3 orig(0,0,0);
  TVector3 tran(0,0,0);
  if (!fIsSingleCyDet) IOADatabase::Get().GeomId().MasterToLocal(fInnerMostLayerNodeId,orig,tran);
  COMETNamedDebug("ICDCWireManager", " (0,0,0) -> (" << tran[0] << "," << tran[1] << "," << tran[2] << ")");

  TVector3 master(0,0,0); // master position
  TVector3 end0master(0,0,0);
  TVector3 end1master(0,0,0);
  int found = 0;

  for (unsigned int lay = 0; lay < fWireToNodeId.size(); lay++) {
    COMETNamedDebug("ICDCWireManager", " #of wires = " << fWireToNodeId.at(lay).size() << " @ " << lay << "-th layer");
    for (unsigned int cell = 0; cell < fWireToNodeId.at(lay).size(); cell++) {
      int nodeId = fWireToNodeId.at(lay).at(cell);
      cdcNode = IOADatabase::Get().GeomId().GetNode(nodeId);
      if (!cdcNode){
          COMETError("Cannot find node for CDC wire "<<lay<<" "<<cell<<" with node id = "<<nodeId);
          continue;
      }
      std::string name(cdcNode->GetName());

      /// Sense wire 
      if (name.find("SenseWire") != std::string::npos) {
        // Record this wire ID
        int thisWire = GetWireId(lay, cell);
        TVector3 nodeSize = IOADatabase::Get().GeomId().NodeSize(cdcNode,IGeomIdManager::kTube);

        // Create wire info
        ICDCWireInfo wireInfo = ICDCWireInfo();
        wireInfo.SetWireNode(fWireToNodeId.at(lay).at(cell));
        wireInfo.SetLayerId(lay);
        wireInfo.SetWireId(thisWire); 
        wireInfo.SetBoardId(boardMap[lay][cell]);
        wireInfo.SetLength(2.*nodeSize.Z());

        IOADatabase::Get().GeomId().LocalToMaster(nodeId,0,0,0,master);
        wireInfo.SetPosition(master);
        IOADatabase::Get().GeomId().LocalToMasterVect(nodeId,0,0,-nodeSize.Z(),end0master);
        IOADatabase::Get().GeomId().LocalToMasterVect(nodeId,0,0,+nodeSize.Z(),end1master);

        wireInfo.SetEnd0Pos(master+end0master);
        wireInfo.SetEnd1Pos(master+end1master);
        fWireInfoMap[thisWire] = wireInfo;
        found++;
      }
    }
  }

  return found;
}

bool ICDCWireManager::SetWireMap(const std::vector< std::vector< int > >& wiremap) {

  fNumberOfWires = 0;

  fWireToNodeId.resize(0);
  std::vector < int > tmp;
  for (unsigned int i = 0; i < wiremap.size(); i++) {
    tmp.resize(0);
    for (unsigned int j = 0; j < wiremap.at(i).size(); j++) {
      if (wiremap.at(i).at(j)<0) continue;
      tmp.push_back(wiremap.at(i).at(j));
    }
    if (tmp.size()>0) {
      fWireToNodeId.push_back(tmp);
      fFirstWireIndex.push_back(fNumberOfWires);
      fNumberOfWires += fWireToNodeId.at(i).size();
    }
  }

  if (!fNumberOfWires) return false;

  fNumberOfLayers = fWireToNodeId.size();
  return true;
}

void ICDCWireManager::GetBoardMap(std::string rootFile,
                                         std::map<int, std::map<int, int> >& boardMap){
    // Clear the current mapping
    boardMap.clear();
    // Cache our current working directory
    TDirectory* oldDir = TDirectory::CurrentDirectory();
    // Open the ROOT file with the board information
    TFile* boardFile = TFile::Open(rootFile.c_str(), "READ"); 
    // Get the tree with the board information
    TTree* boardTree = (TTree*)boardFile->Get("t");

    // Map the layer and cell ID to the board IDs
    int layerID, cellID, boardID, isSignalWire;
    // Connect the branches to the values
    boardTree->SetBranchAddress("LayerID", &layerID);
    boardTree->SetBranchAddress("CellID", &cellID);
    boardTree->SetBranchAddress("BoardID", &boardID);
    boardTree->SetBranchAddress("isSenseWire", &isSignalWire);

    // Fill the map
    COMETNamedDebug("ICDCWireManager", "Board ID Mapping");
    Int_t n_entries = boardTree->GetEntries();
    for(Int_t i=0; i<boardTree->GetEntries(); i++) {
        // Get the entry
        boardTree->GetEntry(i);

        COMETNamedDebug("ICDCWireManager", "\n..Board ID  : " << boardID <<
                                           "\n..Layer ID  : " << layerID << 
                                           "\n..Cell ID   : " << cellID  << 
                                           "\n..IsSigWire : " << isSignalWire);
        // If its a signal wire, save this mapping
        if (isSignalWire) boardMap[layerID][cellID] = boardID;
    }
    // Close the file
    boardFile->Close();
    // Ensure we are in the same directory
    oldDir->cd();
}

const ICDCWireInfo* ICDCWireManager::GetWireInfo(int wire) const{
    std::map<int,ICDCWireInfo>::const_iterator it;
    it = fWireInfoMap.find(wire);
    if (it == fWireInfoMap.end()) return NULL;
    else return &(it->second);
}

int ICDCWireManager::GetWireNode(int wire) const {
    const ICDCWireInfo* wireInfo = GetWireInfo(wire);
    if (!wireInfo) return -1;
    else return wireInfo->GetWireNode();
}


int ICDCWireManager::GetCellId(int wire) const {
  // Return the cell id as the current wire id minus the first wire in the layer 
  // of the wire
  int cellID = wire - GetFirstWireIndexAt(GetLayer(wire));

  /// Return -1 if cellID is negative.
  if (cellID>=0) return cellID;
  else return -1;
}

int ICDCWireManager::GetLayer(int wire) const {
    const ICDCWireInfo* wireInfo = GetWireInfo(wire);
    if (!wireInfo) return -1;
    else return wireInfo->GetLayerId();
}

int ICDCWireManager::GetBoard(int wire) const {
    const ICDCWireInfo* wireInfo = GetWireInfo(wire);
    if (!wireInfo) return -1;
    else return wireInfo->GetBoardId();
}

double ICDCWireManager::GetWireLength(int wire) const {
    const ICDCWireInfo* wireInfo = GetWireInfo(wire);
    if (!wireInfo) return 1e9;
    else return wireInfo->GetLength();
}

TVector3 ICDCWireManager::GetWirePosition(int wire) const {
    const ICDCWireInfo* wireInfo = GetWireInfo(wire);
    if (!wireInfo) return TVector3(1e9, 1e9, 1e9);
    else return wireInfo->GetPosition();
}

TVector3 ICDCWireManager::GetWireEnd0(int wire) const {
    const ICDCWireInfo* wireInfo = GetWireInfo(wire);
    if (!wireInfo) return TVector3(1e9, 1e9, 1e9);
    else return wireInfo->GetEnd0Pos();
}

TVector3 ICDCWireManager::GetWireEnd1(int wire) const {
    const ICDCWireInfo* wireInfo = GetWireInfo(wire);
    if (!wireInfo) return TVector3(1e9, 1e9, 1e9);
    else return wireInfo->GetEnd1Pos();
}

TVector3 ICDCWireManager::GetWireDirection(int wire) const{
    const ICDCWireInfo* wireInfo = GetWireInfo(wire);
    if (!wireInfo) return TVector3(1e9, 1e9, 1e9);
    else return wireInfo->GetEnd01Unit();
}

TGeoNode* ICDCWireManager::ChannelToModuleNode(int channel) const {

  int wire = ChannelToWire(channel);
  int node = GetWireNode(wire);
  if (node < 0) return NULL;

  std::string name = "";
  TGeoNode * cdcNode = IOADatabase::Get().GeomId().GetNode(node);
  if (cdcNode){
      name = cdcNode->GetName();
  }
  else{
      COMETWarn("Cannot find TGeoNode for wire "<<wire);
  }

  if (name.find("SenseWire") == std::string::npos) return NULL;
  return cdcNode;
}

bool ICDCWireManager::ChannelToGlobalPosition(int channel, TVector3& global) const {

  int wire = ChannelToWire(channel);
  int node = GetWireNode(wire);
  if (node < 0) return NULL;

  const ICDCWireInfo * wireInfo = GetWireInfo(wire);
  if (!wireInfo){
      COMETError("Cannot get ICDCWireInfo for wire "<<wire);
  }
  IOADatabase::Get().GeomId().LocalToMaster(wireInfo->GetWireNode(),0,0,0,global);
  return true;
}

ECDCHitVolume ICDCWireManager::GlobalPositionToChannel(const TVector3& global, int& channel) const {

  channel  = -1;
  TGeoManager* geom = IOADatabase::Get().GetGeometry();  
  TGeoNode* cdcNode = IOADatabase::Get().GeomId().FindNodeAndEnter(global.X(), global.Y(), global.Z());
  std::string cdcName(geom->GetPath());
  std::string nodeName = cdcNode->GetName();
  std::string volName = cdcName + "/" + nodeName;
  geom->PopPath();

  // Make sure that the node is a CDC layer.
  size_t name_pos = volName.find("CDCSenseLayer");
  if (name_pos == std::string::npos) {
    return kCDCOut;
  }

  // Find which layer it is in by name
  int layer = -1;
  size_t underscore1 = volName.find("_", name_pos);
  size_t underscore2 = volName.find("_", underscore1+1);

  std::string layer_str = volName.substr(underscore1 + 1, 
          underscore2 - (underscore1 + 1));

  try {
      layer = std::stoi(layer_str.c_str());
  } catch(std::invalid_argument&) {
      return kCDCOut;
  }

  // Generate the local channel number.
  TVector3 pos = IGeomInfo::CDC().GlobalPosition_to_LocalPosition(global);
  COMETNamedDebug("ICDCWireManager", "GlobalPositionToChannel\n global position, = ("
                  << global.x() << "," << global.y() << "," << global.z() << ")\n local position, = ("
                  << pos[0] << "," << pos[1] << "," << pos[2] << "), layer = " << layer
                  << ", r/phi = " << std::sqrt(pos[0]*pos[0]+pos[1]*pos[1]) << "," << std::atan2(pos[1],pos[0])*180./unit::pi);
  if (!LocalPositionToChannel(pos, layer, channel)) return kCDCOut;

  ECDCHitVolume volume = kCDCCell;
  if (volName.find("SenseWire") != std::string::npos){
      volume = kCDCSenseWire;
  }
  else if (volName.find("FieldWire") != std::string::npos){
      volume = kCDCFieldWire;
  }

  return volume;
}

bool ICDCWireManager::LocalPositionToChannel(const TVector3& local, int layer, int& channel) const {
  // Get the wire ID
  int wireID = GetWireNumber(layer, local);
  // Get the channel number
  channel = WireToChannel(wireID);
  COMETNamedDebug("ICDCWireManager", "  channel = " << channel);
  return true;
}

bool ICDCWireManager::GetDistanceFromWire(const TVector3& global, int wire, TVector3& local) const {
  // Set up dummies to pass by reference

  const ICDCWireInfo * wireInfo = GetWireInfo(wire);
  if (!wireInfo){
      COMETError("Cannot get ICDCWireInfo for wire "<<wire);
  }
  IOADatabase::Get().GeomId().MasterToLocal(wireInfo->GetWireNode(), global, local);

  COMETNamedDebug("ICDCWireManager", " Position w.r.t wire " << wire << " = (" << local[0] << "," << local[1] << "," << local[2] << ")");

  return true;
}

bool ICDCWireManager::GetGlobalFromWireDistance(const TVector3& local, int wire, TVector3& global) const {

  const ICDCWireInfo * wireInfo = GetWireInfo(wire);
  if (!wireInfo){
      COMETError("Cannot get ICDCWireInfo for wire "<<wire);
      return false;
  }
  IOADatabase::Get().GeomId().LocalToMaster(wireInfo->GetWireNode(), local, global);
  return true;
}

bool ICDCWireManager::CalcSegToWire(const TVector3& vPrePoint, const TVector3& vPostPoint, int wire, TVector3& vPocaW, TVector3& vPocaT, int & LR) const {
  ///-# Get wire direction vwireDir (End0 -> End1, upstream to downstream) and segment vector vSegVect, direction vSegDir (from pre-point to post-point)
  TVector3 vWireEnd0(GetWireEnd0(wire));
  TVector3 vWireEnd1(GetWireEnd1(wire));
  TVector3 vWireDir = (vWireEnd1-vWireEnd0).Unit();
  TVector3 vSegVect = (vPostPoint - vPrePoint);
  TVector3 vSegDir = vSegVect.Unit();

  ///-# Get the doca (on seg-line) direction (vDOCADir) by seg x wire (pointing from wire to seg if it's right handed helix)
  ///   and the doca (on seg-line) vector (vDOCA) from poca on wire to poca on seg-line by projecting the vector wireEnd0-to-prePoint onto the line of vDOCADir
  TVector3 vDOCADir = (vSegDir.Cross(vWireDir)).Unit();
  TVector3 vDOCA = ((vPrePoint - vWireEnd0).Dot(vDOCADir))*vDOCADir;
  ///   * Get left right flag:
  ///     vDOCADir.Dot(vDOCA) > 0? right, LR =  1
  ///                         < 0? left,  LR = -1
  ///     TODO: should we consider a threshold? mark LR as 0 if it'd difficult to tell ...
  LR = vDOCADir.Dot(vDOCA)>0?1:-1;

  ///-# Get the vertical-to-wire direction vSegVert2WireDir (vertical to the wire and on the common plane) by subtracting the component of vSegDir on vWireDir line from vSegDir
  TVector3 vSegVert2WireDir = (vSegDir-(vSegDir.Dot(vWireDir))*vWireDir).Unit();

  ///-# Get the coordinate of pre-point and post-point along vSegVert2WireDir, taking the poca on seg-line/wire (same on this line) as 0.
  double xPre = (vPrePoint - vWireEnd0).Dot(vSegVert2WireDir);
  double xPost = (vPostPoint - vWireEnd0).Dot(vSegVert2WireDir);

  ///-# Tell the position of poca on seg-line and get the restricted poca on segment
  TVector3 vDOCAres = vDOCA; // the restricted doca vector
  ///   * If pre-point and post-point on the same side of the poca on seg-line, poca out of segment; give restricted poca then
  if (xPre*xPost>=0){
    ///     * seg-line poca before pre-point, restricted at pre-point
    if (xPre>0){
      vPocaT = vPrePoint;
      vDOCAres += xPre*vSegVert2WireDir;
    }
    ///     * seg-line poca after post-point, restricted at post-point
    else{
      vPocaT = vPostPoint;
      vDOCAres += xPost*vSegVert2WireDir;
    }
  }
  ///   * If pre-point and post-point not on the same side, so seg-line poca in segment; give the seg-line poca
  else{
    vPocaT = vPrePoint + (-xPre)/(xPost-xPre)*vSegVect;
  }

  ///-# Get the segment poca on wire: vPocaW = vPocaT-vDOCAres;
  vPocaW = vPocaT-vDOCAres;

  return true;
}


bool ICDCWireManager::GetWirePositionXY(int wire, double zpos, TVector2& xypos) const {

  TVector3 locEnd0(0,0,0);
  TVector3 locEnd1(0,0,0);

  if (!fIsSingleCyDet) {
    IOADatabase::Get().GeomId().MasterToLocal(fInnerMostLayerNodeId, GetWireEnd0(wire), locEnd0);
    IOADatabase::Get().GeomId().MasterToLocal(fInnerMostLayerNodeId, GetWireEnd1(wire), locEnd1);
  } else {
  }

  COMETNamedDebug("ICDCWireManager", "   local wire End0 = " << locEnd0[0] << "," << locEnd0[1] << " " << locEnd0[2]);
  COMETNamedDebug("ICDCWireManager", "   local wire End1 = " << locEnd1[0] << "," << locEnd1[1] << " " << locEnd1[2]);

  double ratio = 0;
  if (!fabs(locEnd0[2] - locEnd1[2])) return false;
  ratio = fabs(zpos - locEnd0[2])/fabs(locEnd1[2] - locEnd0[2]);

  double xpos = ratio * (locEnd1[0] - locEnd0[0]) + locEnd0[0];
  double ypos = ratio * (locEnd1[1] - locEnd0[1]) + locEnd0[1];

  xypos.Set(xpos, ypos);
  return true;
}

int ICDCWireManager::GetWireNumber(int layer, const TVector3& local) const {

  // Get the XY position of the first wire in the layer at the given z position
  int wirenum = GetFirstWireIndexAt(layer);
  TVector2 xypos;
  GetWirePositionXY(wirenum, local.Z(), xypos);

  // Find the wire in this layer with the smallest angular seperation from the 
  // needed position
  // TODO this may disagree with how channel ID's are assigned.  The assumption 
  // here is the channel ID increases in the direction of positive phi (which 
  // may be a safe assumption, but could also introduce bugs)
  double phiHit = local.Phi();
  double phi0   = atan2(xypos.Y(), xypos.X());
  double dphi   = unit::twopi/((double)GetNumberOfWiresPerLayer(layer));
  double phi    = phi0;
  int wireId = -1;
  for (int iwire = wirenum; iwire < wirenum+GetNumberOfWiresPerLayer(layer); iwire++) {
    if (fabs(phiHit-phi)<dphi/2.) {
      wireId = iwire;
      break;
    } else if (fabs(phiHit-phi-unit::twopi)<dphi/2.) {
      wireId = iwire;
      break;
    } else if (fabs(phiHit-phi+unit::twopi)<dphi/2.) {
      wireId = iwire;
      break;
    }
    phi += dphi;
  }

  return wireId;
}