#include <iostream>
#include <cmath>
#include <algorithm>

#include <TGeoManager.h>
#include <TGeoNode.h>
#include <TGeoArb8.h>
#include <TVector3.h>
#include <TRandom.h>

#include <HEPUnits.hxx>

#include <ICOMETLog.hxx>
#include "IGeomVisitor.hxx"
#include "ICDCGeom.hxx"
#include "ICDCWireManager.hxx"

#include <IOADatabase.hxx>
#include <COMETGeomId.hxx>
#include <COMETGeomIdDef.hxx>
#include <ICDCGeomId.hxx>
#include <IGeomIdManager.hxx>

namespace COMET {
  /// For information see the discription of the base class 'IGeomVisitor'
  class ICDCGeomVisitor: public COMET::IGeomVisitor {
  public:
    explicit ICDCGeomVisitor(COMET::ICDCGeom* geom):
        fDetSolNodeId(0)
       ,fInnerMostLayerNodeId(0)
    {
      fCDCGeom = geom;
    }
    bool VisitNode(int d, const std::string& name, const TGeoNode* node);

    COMET::ICDCGeom::IGeoNodeList_t fSenseLayers; /// List of TGeoNode for sense layers, int key is the size of this node (to be replaced to the layer Id)
    COMET::ICDCGeom::IGeoNodeList_t fSenseWires;  /// List of TGeoNode for sense wires, int key is the size of this node (to be replaced to the wire Id)
    COMET::ICDCGeom::IGeoNodeList_t fFieldWires;  /// List of TGeoNode for field wires, int key is the size of this node (to be replaced to the wire Id)
    std::map<int,std::string > CDCNames; /// This save all paths of volumes we want to pass to the RecPack.
    int fDetSolNodeId;
    int fInnerMostLayerNodeId;

  private:
    COMET::ICDCGeom* fCDCGeom;
  };
};


using namespace COMET;

bool ICDCGeomVisitor::VisitNode(int, const std::string& name, const TGeoNode* node)
{
//COMETLog("ICDCGeomVisitor::VisitNode, visiting :" << name);

  /// Get the ID of the Detector Solenoid
  if (name == "/comet_1/DetectorSolenoid_0") {
    gGeoManager->cd(name.c_str());
    fDetSolNodeId = gGeoManager->GetCurrentNodeId();
  }

  /// If the node isn't in the CyDet, skip this node.
  if (name.find("/CylindricalDetector") == std::string::npos) return true;

  /// If the node isn't in the CDC, skip this node.
  if (name.find("/CylindricalDriftChamber") == std::string::npos) return true;

  /// If the node isn't in the CDC sense layer, skip this node.
  if (name.find("/CDCSenseLayer") == std::string::npos) return true;

  int nodeID = -1;
  int index  = CDCNames.size();
  CDCNames[index] = name;

  gGeoManager->cd(name.c_str());
  nodeID = gGeoManager->GetCurrentNodeId();
  std::pair<int,const TGeoNode*> tmp(nodeID,node);

  // If it is not a sense wire, it must be a sense layer
  if ( name.find("SenseWire") == std::string::npos && name.find("FieldWire") == std::string::npos) { // Layer
    index = fSenseLayers.size();
    fSenseLayers[index] = tmp;
    // If the node is the first CDC sense layer
    if (name.find("/CDCSenseLayer_0_") != std::string::npos) fInnerMostLayerNodeId = nodeID;
  } else {
    if ( name.find("SenseWire") != std::string::npos ) { // Sense wire
      index = fSenseWires.size();
      fSenseWires[index] = tmp;
    } else if ( name.find("FieldWire") != std::string::npos ) { // Field wire
      index = fFieldWires.size();
      fFieldWires[index] = tmp;
    }
  }

  return true;
}


ICDCGeom::ICDCGeom()
:IGeomBase()
,fCDCWireManager(NULL)
,fDetSolNodeId(-1)
{
  COMETNamedTrace("ICDCGeom","ICDCGeom()");
  fWireMap.resize(kMaxNumLayers, std::vector<int>(kMaxNumWires, -999));
}


ICDCGeom::~ICDCGeom() {
  Clear();
}


void ICDCGeom::Fill() {
  COMETNamedTrace("ICDCGeom","Fill()");
  fCDCWireManager = new ICDCWireManager();
  ICDCGeomVisitor visitor(this);

  visitor.VisitGeometry();

  fSenseLayers  = visitor.fSenseLayers;
  fSenseWires   = visitor.fSenseWires;
  fFieldWires   = visitor.fFieldWires;
  fDetSolNodeId = visitor.fDetSolNodeId;
  fInnerMostLayerNodeId = visitor.fInnerMostLayerNodeId;

  // New information needed by oaRecPack. The path names will be saved during the search. 
  for (unsigned int i=0; i< visitor.CDCNames.size();i++) {
    IGeomModuleBase* myModule = new IGeomModuleBase(visitor.CDCNames[i]);
    double x0 = 20*unit::meter; 
    myModule->SetX0(x0);
    fModules.push_back(myModule);
  }

  BuildWireMap();
  fCDCWireManager->Init(fWireMap, fDetSolNodeId, fInnerMostLayerNodeId);
}


void ICDCGeom::ls(const char *opt) const {

  COMETInfo( "// --------------------------------------------------------------------- "); 
  COMETInfo( "// CDC geometry "); 
  COMETInfo( "// --------------------------------------------------------------------- "); 
  COMETInfo( "//  Number of Layers     = " << GetNumberOfLayers());
  COMETInfo( "//  Number of SenseWires = " << GetNumberOfWires()); 
  COMETInfo( "// --------------------------------------------------------------------- ");

  if ( strcmp(opt,"VERBOSE") ) return ; 
}


void ICDCGeom::Clear(void)
{
  if (fCDCWireManager != NULL) {
    delete fCDCWireManager;
  }

  std::vector<IGeomModuleBase*>::iterator modules_it  = fModules.begin();
  std::vector<IGeomModuleBase*>::iterator modules_end = fModules.end();
  for ( ; modules_it != modules_end; ++modules_it)
  {
    if (*modules_it) delete (*modules_it);
  }
  fModules.clear();
}


int ICDCGeom::GetNumberOfLayers() const {
    return fCDCWireManager->GetNumberOfLayers();
}

int ICDCGeom::GetNumberOfWires() const {
    return fCDCWireManager->GetNumberOfWires();
}

int ICDCGeom::GetNumberOfWiresPerLayer(int layer) const {
    return fCDCWireManager->GetNumberOfWiresPerLayer(layer);
}

int ICDCGeom::GetFirstWireIndexAt(int layer) const{
    return fCDCWireManager->GetFirstWireIndexAt(layer);
}

/// Get the list of wires in a layer 
bool ICDCGeom::GetWiresInLayer(int layer, std::vector<int>& list) const
{
    // check if illegal layer is used
    if (layer>=GetNumberOfLayers() || layer<0) return false;

    int offset = fCDCWireManager->GetFirstWireIndexAt(layer);

    list.resize(0);
    for (int i=0;i<GetNumberOfWiresPerLayer(layer);i++) list.push_back(i+offset);
    return true;
}

int ICDCGeom::GetWireNode(int wire) const{
    return fCDCWireManager->GetWireNode(wire);
};

int ICDCGeom::GetCellId(int wire) const {
    return fCDCWireManager->GetCellId(wire);
};

int ICDCGeom::GetLayer(int wire) const {
    return fCDCWireManager->GetLayer(wire);
};

int ICDCGeom::GetBoard(int wire) const {
    return fCDCWireManager->GetBoard(wire);
};

double ICDCGeom::GetWireLength(int wire) const {
    return fCDCWireManager->GetWireLength(wire);
};

TVector3 ICDCGeom::GetWirePosition(int wire) const {
    return fCDCWireManager->GetWirePosition(wire);
};

TVector3 ICDCGeom::GetWireEnd0(int wire) const {
    return fCDCWireManager->GetWireEnd0(wire);
};

TVector3 ICDCGeom::GetWireEnd1(int wire) const {
    return fCDCWireManager->GetWireEnd1(wire);
};

TVector3 ICDCGeom::GetWireDirection(int wire) const {
    return fCDCWireManager->GetWireDirection(wire);
}

int ICDCGeom::GetWireId(int layer, int cellID) const{
    return fCDCWireManager->GetWireId(layer, cellID);
}

int ICDCGeom::ChannelToWire(int channel) const {
    return fCDCWireManager->ChannelToWire(channel);
};

int ICDCGeom::WireToChannel(int wire) const {
    return fCDCWireManager->WireToChannel(wire);
};

const TGeoNode* ICDCGeom::ChannelToModuleNode(int channel) const{
    return fCDCWireManager->ChannelToModuleNode(channel);
};

bool ICDCGeom::ChannelToGlobalPosition(int channel, TVector3& global) const{
    return fCDCWireManager->ChannelToGlobalPosition(channel,global);
}

ECDCHitVolume ICDCGeom::GlobalPositionToChannel(const TVector3& global, 
        int& channel) const{
    return  fCDCWireManager->GlobalPositionToChannel(global, channel);
}

bool ICDCGeom::GetDistanceFromWire(const TVector3& global, 
        int wire, TVector3& local) const{
    return fCDCWireManager->GetDistanceFromWire(global, wire, local);
}

bool ICDCGeom::GetGlobalFromWireDistance(const TVector3& local, 
        int wire, TVector3& global) const{
    return fCDCWireManager->GetGlobalFromWireDistance(local, wire, global);
}

bool ICDCGeom::GetWirePositionXY(int wire, double zpos, TVector2& xypos) const {
    return fCDCWireManager->GetWirePositionXY(wire, zpos, xypos);
}

bool ICDCGeom::CalcSegToWire(const TVector3& pre, const TVector3& post, 
        int wire, TVector3& pocaW, TVector3& pocaT, int & lr) const{
    return fCDCWireManager->CalcSegToWire(pre,post,wire,pocaW,pocaT,lr);
}

double ICDCGeom::GetInOut(const TVector3& position, const TVector3& momentum) const {
    TVector2 local_xy = GlobalPosition_to_LocalPosition(position).XYvector().Unit();
    TVector2 local_pxpy = GlobalMomentum_to_LocalMomentum(momentum).XYvector().Unit();

    return local_xy * local_pxpy;
}

bool ICDCGeom::ChannelToGeomId(int channel, IGeometryId& id) const {

  int wire   = ChannelToWire(channel);
  int layer  = GetLayer(wire);
  int active = 1;
  int isLayer = 0;

  id = GeomId::CDC::SenseWireId(isLayer, layer, wire, active);
  return id.IsValid();
}

const TGeoNode* ICDCGeom::GeomIdToModuleNode(const IGeometryId& id) const {
  int wireId = GeomIdToWire(id);
  if (wireId == -1) return NULL;
  int channel = ChannelToWire(wireId);
  return ChannelToModuleNode(channel);
}


bool ICDCGeom::GeomIdToGlobalPosition(const IGeometryId& id , TVector3& global) const {
 
  int wire = GeomIdToWire(id);
  int node = GeomIdToNodeId(id);
  if (node == -1 || wire == -1) return false;

  bool ok = fCDCWireManager->ChannelToGlobalPosition(wire,global);
  return ok;
}


bool ICDCGeom::GlobalPositionToGeomId(const TVector3& global, IGeometryId& id) const {
  if (!IOADatabase::Get().GeomId().GetGeometryId(global.X(), global.Y(), global.Z(), id)) return false;

  if (!GeomId::CDC::IsCDC(id)) return false;
  int channel = -1;

  if (fCDCWireManager->GlobalPositionToChannel(global, channel) == kCDCOut) return false;
  return ChannelToGeomId(channel, id);
}


int ICDCGeom::GeomIdToLayer(const IGeometryId& id) const {
    // If it is in the CDC, it has a layer ID
    if (GeomId::CDC::IsCDC(id)) return GeomId::CDC::GetLayerId(id);
    // If not, return -1
    else return -1;
}

int ICDCGeom::GeomIdToWire(const IGeometryId& id) const {
    // If it is an active sensitive wire, return the wire ID field of the 
    // geometry ID
    if (GeomId::CDC::IsActiveSenseWire(id)) return GeomId::CDC::GetWireId(id);
    // Otherwise, return -1
    else return -1;
}

int ICDCGeom::GeomIdToCell(const IGeometryId& id) const {
  int wire  = GeomIdToWire(id);
  return fCDCWireManager->GetCellId(wire);
}

bool ICDCGeom::GetGeometryInfo(const IGeometryId& id, int& layer, int& wire) const {
  wire = GeomIdToWire(id);
  layer = GeomIdToLayer(id);
  if (wire == -1 || layer == -1) return false;
  else return true;
}

int ICDCGeom::GeomIdToNodeId(const IGeometryId& id) const {
  int wire  = GeomIdToWire(id);
  return GetWireNode(wire);
}

TVector3 ICDCGeom::GlobalPosition_to_LocalPosition(const TVector3 & globalPos) const {
    TVector3 localPos(0,0,0);
    IOADatabase::Get().GeomId().MasterToLocal(fInnerMostLayerNodeId,globalPos,localPos);
    return localPos;
}

TVector3 ICDCGeom::LocalPosition_to_GlobalPosition(const TVector3 & localPos) const {
    TVector3 globalPos(0,0,0);
    IOADatabase::Get().GeomId().LocalToMaster(fInnerMostLayerNodeId,localPos,globalPos);
    return globalPos;
}

TVector3 ICDCGeom::GlobalMomentum_to_LocalMomentum(const TVector3 & globalMom) const {
    TVector3 localMom(0,0,0);
    IOADatabase::Get().GeomId().MasterToLocalVect(fInnerMostLayerNodeId,globalMom,localMom);
    return localMom;
}

TVector3 ICDCGeom::LocalMomentum_to_GlobalMomentum(const TVector3 & localMom) const {

    TVector3 globalMom(0,0,0);
    IOADatabase::Get().GeomId().LocalToMasterVect(fInnerMostLayerNodeId,localMom,globalMom);
    return globalMom;
}

void ICDCGeom::BuildWireMap() {
  COMETNamedTrace("ICDCGeom","BuildWireMap()");

  IGeoNodeList_t nodes = fSenseWires;
  IGeoNodeList_t::iterator it;

  std::vector<  bool > used(kMaxNumWires, false);
  bool found = false;

  for(it = nodes.begin(); it != nodes.end(); it++){
    int node_id = (*it).second.first;
    TString path = IOADatabase::Get().GeomId().FullNodePath(node_id);

    found = false;
    if (!path.Contains("SenseWire")) continue;

    for (int iwire = 0; iwire < kMaxNumWires; iwire++) {

      if (used[iwire]) continue;

      for (int ilayer = 0; ilayer < kMaxNumLayers; ilayer++) {

        char slayer[256];
        sprintf(slayer, "/CDCSenseLayer_%d_0", ilayer);

        if (!path.Contains(slayer)) continue;

        char swire[256];
        sprintf(swire,  "/SenseWire_%d_0", iwire);
        if (path.Contains(swire)) {

          fWireMap[ilayer][iwire] = node_id;

          COMETNamedInfo("ICDCGeom", path);
          COMETNamedInfo("ICDCGeom", " CDC LAYER = " << slayer << ", WIRE = " << swire << " --> " << node_id); 
          COMETNamedInfo("ICDCGeom", " CDC LAYER INDEX = " << ilayer << ", WIRE INDEX = " << iwire << " --> " << node_id); 

          used[iwire] = true;
          found = true;
          break;
        }
      }
      if (found) break;
    }
  }
}