GenericStateGraph.h

Go to the documentation of this file.

#ifndef PPL_STATE_GRAPH_H_
#define PPL_STATE_GRAPH_H_
 
#include "MPProblem/Environment/Environment.h"
#include "Utilities/PMPLExceptions.h"
#include "Utilities/RuntimeUtils.h"
 
#ifdef _PARALLEL
#include <containers/graph/dynamic_graph.hpp>
//#include "graph/view/graph_view_property_adaptor.h"
//#include <containers/graph/view/graph_view_property_adaptor.hpp>
#include <containers/sequential/graph/algorithms/connected_components.h>
#include <containers/graph/algorithms/graph_io.hpp>
#else
#include <containers/sequential/graph/graph.h>
#include <containers/sequential/graph/graph_util.h>
#include <containers/sequential/graph/vertex_iterator_adaptor.h>
#include <containers/sequential/graph/algorithms/connected_components.h>
#include <containers/sequential/graph/algorithms/graph_input_output.h>
#endif
 
#ifndef INVALID_VID
#define INVALID_VID (std::numeric_limits<size_t>::max())
#endif
 
#ifndef INVALID_EID
#define INVALID_EID (std::numeric_limits<size_t>::max())
#endif
 
// #include "MPLibrary/MPBaseObject.h"
#include "Utilities/CCTracker.h"
 
#include <fstream>
#include <functional>
#include <iostream>
#include <string>
#include <unordered_map>
#include <unordered_set>
#include <vector>
 
class Robot;
 
 
template <typename Vertex, typename Edge>
class GenericStateGraph: public
#ifdef _PARALLEL
    stapl::dynamic_graph<stapl::DIRECTED, stapl::NONMULTIEDGES, Vertex, Edge>
#else
    stapl::sequential::graph<stapl::DIRECTED, stapl::NONMULTIEDGES, Vertex,
        Edge>
#endif
{
 
  public:
 
 
    using STAPLGraph =
#ifdef _PARALLEL
    stapl::dynamic_graph<stapl::DIRECTED,stapl::NONMULTIEDGES,Vertex, Edge>
#else
    stapl::sequential::graph<stapl::DIRECTED, stapl::NONMULTIEDGES, Vertex, Edge>
#endif
    ;
 
    // Descriptor types.
    typedef typename STAPLGraph::vertex_descriptor        VID;
    typedef typename STAPLGraph::edge_descriptor          EID;
    typedef typename EID::edge_id_type                    EdgeID;
    typedef typename std::unordered_set<VID>              VertexSet;
 
    // Iterator types.
    typedef typename STAPLGraph::vertex_iterator          VI;
    typedef typename STAPLGraph::adj_edge_iterator        EI;
    typedef typename STAPLGraph::const_vertex_iterator    CVI;
    typedef typename STAPLGraph::const_adj_edge_iterator  CEI;
 
    // Property types.
    typedef typename STAPLGraph::vertex_property          VP;
    typedef typename STAPLGraph::edge_property            EP;
    // Useful names for the property types.
    typedef Vertex                                        CfgType;
    typedef Edge                                          EdgeType;
 
    typedef stapl::sequential::vector_property_map<STAPLGraph, size_t> ColorMap;
 
    // Hook function types.
    typedef std::function<void(VI)> VertexHook;
    typedef std::function<void(EI)> EdgeHook;
    enum class HookType {AddVertex, DeleteVertex, AddEdge, DeleteEdge};
 
    // CC Tracker.
    typedef CCTracker<GenericStateGraph<Vertex, Edge>> CCTrackerType;
 
 
    GenericStateGraph();
    GenericStateGraph(Robot* const _r);
 
 
    GenericStateGraph(const GenericStateGraph& _r);
    GenericStateGraph(GenericStateGraph&& _r);
 
    GenericStateGraph& operator=(const GenericStateGraph& _r);
    GenericStateGraph& operator=(GenericStateGraph&& _r);
 
 
    bool operator==(const GenericStateGraph& _r) const noexcept;
    bool operator!=(const GenericStateGraph& _r) const noexcept;
 
 
    virtual VID AddVertex(const Vertex& _v) noexcept;
 
    virtual VID AddVertex(const VID _vid, const Vertex& _v) noexcept;
 
    virtual VID AddDuplicateVertex(const Vertex& _v) noexcept;
 
    virtual void DeleteVertex(const VID _v) noexcept;
 
    virtual EID AddEdge(const VID _source, const VID _target, const Edge& _w)
        noexcept;
 
    virtual std::pair<EID, EID> AddEdge(const VID _source, const VID _target,
        const std::pair<Edge, Edge>& _w) noexcept;
 
    virtual EID AddEdge(const EID _eid, const Edge& _w) noexcept;
 
    virtual EID AddEdge(const VID _source, const VID _target) noexcept;
 
    virtual void DeleteEdge(const VID _source, const VID _target) noexcept;
 
    virtual void DeleteEdge(EI _iterator) noexcept;
 
    void SetRobot(Robot* const _r) noexcept;
 
    void AppendRoadmap(const GenericStateGraph& _r);
 
    void SetCCTracker(StatClass* const _stats = nullptr);
 
 
    size_t Size() const noexcept;
 
    bool IsVertex(const VID _vid) const noexcept;
 
    bool IsVertex(const Vertex& _v) const noexcept;
 
    bool IsVertex(const Vertex& _v, CVI& _vi) const noexcept;
 
    bool IsEdge(const VID _source, const VID _target) const noexcept;
 
    template <typename T>
    VID GetVID(const T& _t) const noexcept;
    VID GetVID(const VI& _t) const noexcept;
    VID GetVID(const Vertex& _t) const noexcept;
 
    const VertexSet& GetPredecessors(const VID _vid) const noexcept;
 
    VID GetLastVID() const noexcept;
 
    size_t GetTimestamp() const noexcept;
 
    const VertexSet& GetAllVIDs() const noexcept;
 
 
    Robot* GetRobot() const noexcept;
 
    CCTrackerType* GetCCTracker() const noexcept;
 
    template <typename T>
    VP& GetVertex(T& _t) noexcept;
    VP& GetVertex(VI& _t) noexcept;
    VP& GetVertex(VID _t) noexcept;
 
    template <typename T>
    const VP& GetVertex(T& _t) const noexcept;
    const VP& GetVertex(CVI& _t) const noexcept;
    const VP& GetVertex(VID _t) const noexcept;
 
    std::vector<VID> GetChildren(const VID _vid) const noexcept;
 
    size_t GetInDegree(const VID _vid) noexcept;
 
    std::vector<EI> FindInboundEdges(const VID _vid);
 
    bool GetEdge(const VID _source, const VID _target, EI& _ei) noexcept;
 
    bool GetEdge(const VID _source, const VID _target, CEI& _ei) const noexcept;
 
    EP& GetEdge(const VID _source, const VID _target) noexcept;
    EP& GetEdge(const EID _descriptor) noexcept;
 
 
    bool IsHook(const HookType, const std::string& _label) const;
 
    void InstallHook(const HookType _type, const std::string& _label,
        const VertexHook& _h);
 
    void InstallHook(const HookType _type, const std::string& _label,
        const EdgeHook& _h);
 
    void RemoveHook(const HookType _type, const std::string& _label);
 
    void DisableHooks() noexcept;
 
    void EnableHooks() noexcept;
 
    virtual void ClearHooks() noexcept;
 
 
    //virtual void Read(const std::string& _filename);
 
    // Temporary work-around for calling the add vertex/edge hooks.
    template <typename RG>
    friend void Read(RG* _g, const std::string& _filename);
 
        template <typename RG>
        friend void ReadMessage(RG* _g, const std::string& _msg);
        //void ReadMessage(GenericStateGraph* _g, const std::string& _msg);
    virtual void Write(const std::string& _filename, Environment* _env) const;
 
 
#ifdef _PARALLEL
    // Temporarily wrapper for some graph methods
    // Until full migration and change of names in STAPL is completed
    size_t get_num_edges() {return this->num_edges();}
    size_t get_num_vertices() const {return this->num_vertices();}
    size_t get_degree(const VID& _vd) {return this->find_vertex(_vd)->size();}
    size_t get_out_degree(const VID& _vd) {return this->get_degree(_vd);}
#endif
 
  protected:
 
 
    using STAPLGraph::add_vertex;
    using STAPLGraph::add_edge;
    using STAPLGraph::delete_vertex;
    using STAPLGraph::delete_edge;
 
 
    void ExecuteAddVertexHooks(const VI _iterator) noexcept;
 
    void ExecuteDeleteVertexHooks(const VI _iterator) noexcept;
 
    void ExecuteAddEdgeHooks(const EI _iterator) noexcept;
 
    void ExecuteDeleteEdgeHooks(const EI _iterator) noexcept;
 
    std::string ToString(const HookType& _t) const noexcept;
 
 
    Robot* m_robot{nullptr};  
 
    size_t m_timestamp{0};    
 
    bool m_enableHooks{true}; 
 
    std::unordered_map<std::string, VertexHook> m_addVertexHooks;
    std::unordered_map<std::string, VertexHook> m_deleteVertexHooks;
 
    std::unordered_map<std::string, EdgeHook> m_addEdgeHooks;
    std::unordered_map<std::string, EdgeHook> m_deleteEdgeHooks;
 
    std::unique_ptr<CCTrackerType> m_ccTracker;
 
    std::unordered_map<VID, VertexSet> m_predecessors;
 
    VertexSet m_allVIDs;
 
 
};
 
/*------------------------------- Construction -------------------------------*/
 
template <typename Vertex, typename Edge>
GenericStateGraph<Vertex, Edge>::
GenericStateGraph() { }
 
template <typename Vertex, typename Edge>
GenericStateGraph<Vertex, Edge>::
GenericStateGraph(Robot* const _r) : m_robot(_r) { }
 
/*------------------------------ Move and Copy -------------------------------*/
 
template <typename Vertex, typename Edge>
GenericStateGraph<Vertex, Edge>::
GenericStateGraph(const GenericStateGraph& _r) : GenericStateGraph(_r.m_robot) {
  *this = _r;
}
 
 
template <typename Vertex, typename Edge>
GenericStateGraph<Vertex, Edge>::
GenericStateGraph(GenericStateGraph&& _r) : GenericStateGraph(_r.m_robot) {
  *this = std::move(_r);
}
 
 
template <typename Vertex, typename Edge>
GenericStateGraph<Vertex, Edge>&
GenericStateGraph<Vertex, Edge>::
operator=(const GenericStateGraph& _r) {
  // Clear any hooks installed on this map.
  ClearHooks();
 
  // Copy the stapl graph and our add-ons.
  STAPLGraph::operator=(_r);
  m_robot        = _r.m_robot;
  m_timestamp    = _r.m_timestamp;
  m_predecessors = _r.m_predecessors;
  m_allVIDs      = _r.m_allVIDs;
 
  // If the other graph had a CC tracker, copy it and point at this roadmap.
  if(_r.m_ccTracker) {
    m_ccTracker.reset(new CCTrackerType(*_r.m_ccTracker));
    m_ccTracker->SetRoadmap(this);
 
        // Old CC tracker hooks are reinstalled in first call.
        // These need to be cleared and the new CC tracker hooks installed.
        ClearHooks();
  }
 
  return *this;
}
 
 
template <typename Vertex, typename Edge>
GenericStateGraph<Vertex, Edge>&
GenericStateGraph<Vertex, Edge>::
operator=(GenericStateGraph&& _r) {
  // Clear any hooks installed on this map.
  ClearHooks();
 
  // Move the stapl graph and our add-ons.
  STAPLGraph::operator=(std::move(_r));
  m_robot        = _r.m_robot;
  m_timestamp    = _r.m_timestamp;
  m_predecessors = std::move(_r.m_predecessors);
  m_allVIDs      = _r.m_allVIDs;
 
  // If the other graph had a CC tracker, move it and point at this roadmap.
  if(_r.m_ccTracker) {
    m_ccTracker = std::move(_r.m_ccTracker);
    m_ccTracker->SetRoadmap(this);
  }
 
  // Clear any hooks on the other to prevent cross-talk with the CCTracker.
  _r.ClearHooks();
 
  return *this;
}
 
/*------------------------------- Equality -----------------------------------*/
 
template <typename Vertex, typename Edge>
bool
GenericStateGraph<Vertex, Edge>::
operator==(const GenericStateGraph& _r) const noexcept {
  // First do fast checks on addess and sizes.
  if(this == &_r)
    return true;
  if(this->Size() != _r.Size() or this->get_num_edges() != _r.get_num_edges())
    return false;
 
  // Check vertices and edges.
  for(auto va = this->begin(); va != this->end(); ++va) {
    // Find the matching descriptor in _r.
    auto vb = _r.find_vertex(va->descriptor());
 
    // Check that the property and number of edges are the same.
    if(va->property() != vb->property() or va->size() != vb->size())
      return false;
 
    // Check that the edges are the same. Do not assume the edge ordering is
    // the same, only worry about source/target correspondance.
    for(auto ea = va->begin(); ea != va->end(); ++ea) {
      CEI eb;
      if(!_r.GetEdge(ea->source(), ea->target(), eb)
          or ea->property() != eb->property())
        return false;
    }
  }
 
  return  true;
}
 
 
template <typename Vertex, typename Edge>
bool
GenericStateGraph<Vertex, Edge>::
operator!=(const GenericStateGraph& _r) const noexcept {
  return !(*this == _r);
}
 
/*------------------------------- Modifiers ----------------------------------*/
 
template <typename Vertex, typename Edge>
typename GenericStateGraph<Vertex, Edge>::VID
GenericStateGraph<Vertex, Edge>::
AddVertex(const Vertex& _v) noexcept {
  // Find the vertex and ensure it does not already exist.
  CVI vi;
  if(IsVertex(_v, vi)) {
    //std::cerr << "\nGenericStateGraph::AddVertex: vertex " << vi->descriptor()
    //          << " already in graph, not adding again."
    //          << std::endl;
    return vi->descriptor();
  }
 
  // The vertex does not exist. Add it now.
  const VID vid = this->add_vertex(_v);
  m_predecessors[vid];
  m_allVIDs.insert(vid);
  ++m_timestamp;
 
  // Execute post-add hooks and update vizmo debug.
  ExecuteAddVertexHooks(this->find_vertex(vid));
 
  return vid;
}
 
template <typename Vertex, typename Edge>
typename GenericStateGraph<Vertex, Edge>::VID
GenericStateGraph<Vertex, Edge>::
AddVertex(const VID _vid, const Vertex& _v) noexcept {
  // Find the vertex and ensure it does not already exist.
  CVI vi;
  if(IsVertex(_v, vi)) {
    //std::cerr << "\nGenericStateGraph::AddVertex: vertex " << vi->descriptor()
    //          << " already in graph, not adding again."
    //          << std::endl;
    return vi->descriptor();
  }
 
  if(m_allVIDs.count(_vid) > 0) {
    //std::cerr << "\nGenericStateGraph::AddVertex: vertex " << _vid
    //          << " already in graph, not adding again."
    //          << std::endl;
    return _vid;
  }
 
  // The vertex does not exist. Add it now.
  const VID vid = this->add_vertex(_vid, _v);
  m_predecessors[vid];
  m_allVIDs.insert(vid);
  ++m_timestamp;
 
  // Execute post-add hooks and update vizmo debug.
  ExecuteAddVertexHooks(this->find_vertex(vid));
 
  return vid;
}
 
template <typename Vertex, typename Edge>
typename GenericStateGraph<Vertex, Edge>::VID
GenericStateGraph<Vertex, Edge>::
AddDuplicateVertex(const Vertex& _v) noexcept {
 
    const VID vid = this->add_vertex(_v);
  ++m_timestamp;
 
  // Execute post-add hooks and update vizmo debug.
  ExecuteAddVertexHooks(this->find_vertex(vid));
  VDAddNode(_v);
 
  return vid;
}
 
template <typename Vertex, typename Edge>
void
GenericStateGraph<Vertex, Edge>::
DeleteVertex(const VID _v) noexcept {
  // Find the vertex and crash if it doesn't exist.
  VI vi = this->find_vertex(_v);
  if(vi == this->end())
    throw RunTimeException(WHERE) << "VID " << _v << " is not in the graph.";
 
  // We must manually delete the edges rather than letting STAPL do this in
  // order to call the DeleteEdge hooks.
 
  // Delete the outbound edges.
  for(auto edge = vi->begin(); edge != vi->end(); edge = vi->begin())
    DeleteEdge(edge);
 
  // Delete the inbound edges.
  auto predIter = m_predecessors.find(_v);
  VertexSet& preds = predIter->second;
  while(!preds.empty()) {
    const VID predecessor = *preds.begin();
    DeleteEdge(predecessor, _v);
  }
 
  // Execute pre-delete hooks.
  ExecuteDeleteVertexHooks(vi);
 
  // Delete the vertex.
  this->delete_vertex(vi->descriptor());
  m_predecessors.erase(predIter);
  m_allVIDs.erase(_v);
  ++m_timestamp;
}
 
 
template <class Vertex, class Edge>
typename GenericStateGraph<Vertex, Edge>::EID
GenericStateGraph<Vertex, Edge>::
AddEdge(const VID _source, const VID _target, const Edge& _w) noexcept {
  // Let the add_edge function handle checking for existance incase we are using
  // a multigraph.
  const auto edgeDescriptor = this->add_edge(_source, _target, _w);
  const bool notNew = edgeDescriptor.id() == INVALID_EID;
 
  if(notNew) {
    /*
    std::cerr << "\nGenericStateGraph::AddEdge: edge (" << _source << ", "
              << _target << ") already exists, not adding."
              << std::endl;
    */
 
    return edgeDescriptor;
  }
 
  // Add the source as a predecessor of target.
  m_predecessors[_target].insert(_source);
  ++m_timestamp;
 
  // Execute post-add hooks.
  VI vi;
  EI ei;
  this->find_edge(edgeDescriptor, vi, ei);
  ExecuteAddEdgeHooks(ei);
  return edgeDescriptor;
}
 
 
template <class Vertex, class Edge>
typename GenericStateGraph<Vertex, Edge>::EID
GenericStateGraph<Vertex, Edge>::
AddEdge(const VID _source, const VID _target) noexcept {
  // Let the add_edge function handle checking for existance incase we are using
  // a multigraph.
  const auto edgeDescriptor = this->add_edge(_source, _target);
  const bool notNew = edgeDescriptor.id() == INVALID_EID;
 
  if(notNew) {
    /*
    std::cerr << "\nGenericStateGraph::AddEdge: edge (" << _source << ", "
              << _target << ") already exists, not adding."
              << std::endl;
    */
    return edgeDescriptor;
  }
 
  // Add the source as a predecessor of target.
  m_predecessors[_target].insert(_source);
  ++m_timestamp;
 
  // Execute post-add hooks.
  VI vi;
  EI ei;
  this->find_edge(edgeDescriptor, vi, ei);
  ExecuteAddEdgeHooks(ei);
  return edgeDescriptor;
}
 
 
template <class Vertex, class Edge>
std::pair<typename GenericStateGraph<Vertex, Edge>::EID, typename GenericStateGraph<Vertex, Edge>::EID>
GenericStateGraph<Vertex, Edge>::
AddEdge(const VID _source, const VID _target, const std::pair<Edge, Edge>& _w)
    noexcept {
  EID e1 = AddEdge(_source, _target, _w.first);
  EID e2 = AddEdge(_target, _source, _w.second);
  return std::make_pair(e1, e2);
}
 
 
template <class Vertex, class Edge>
typename GenericStateGraph<Vertex, Edge>::EID
GenericStateGraph<Vertex, Edge>::
AddEdge(const EID _eid, const Edge& _w) noexcept {
  // Let the add_edge function handle checking for existance incase we are using
  // a multigraph.
  const auto edgeDescriptor = this->add_edge(_eid, _w);
  const bool notNew = edgeDescriptor.id() == INVALID_EID;
 
  auto source = _eid.source();
  auto target = _eid.target();
 
  if(notNew) {
    /*
    std::cerr << "\nGenericStateGraph::AddEdge: edge (" << source << ", "
              << target << ") already exists, not adding."
              << std::endl;
    */
    return edgeDescriptor;
  }
 
  // Add the source as a predecessor of target.
  m_predecessors[target].insert(source);
  ++m_timestamp;
 
  // Execute post-add hooks.
  VI vi;
  EI ei;
  this->find_edge(edgeDescriptor, vi, ei);
  ExecuteAddEdgeHooks(ei);
  return edgeDescriptor;
}
 
 
template <class Vertex, class Edge>
void
GenericStateGraph<Vertex, Edge>::
DeleteEdge(const VID _source, const VID _target) noexcept {
  // Find the edge and crash if it isn't found.
  const EID edgeDescriptor(_source, _target);
  VI dummy;
  EI edgeIterator;
 
  const bool found = this->find_edge(edgeDescriptor, dummy, edgeIterator);
  if(!found)
    throw RunTimeException(WHERE) << "Edge (" << _source << ", " << _target
                                  << ") does not exist.";
 
  DeleteEdge(edgeIterator);
}
 
 
template <class Vertex, class Edge>
void
GenericStateGraph<Vertex, Edge>::
DeleteEdge(EI _iterator) noexcept {
  // Execute pre-delete hooks.
  const VID source = _iterator->source(),
            target = _iterator->target();
  ExecuteDeleteEdgeHooks(_iterator);
 
  // Delete the edge.
  this->delete_edge(_iterator->descriptor());
  m_predecessors[target].erase(source);
  ++m_timestamp;
}
 
 
template <typename Vertex, typename Edge>
void
GenericStateGraph<Vertex, Edge>::
SetRobot(Robot* const _r) noexcept {
  m_robot = _r;
  for(VI vi = this->begin(); vi != this->end(); ++vi)
    vi->property().SetRobot(_r);
}
 
 
template <typename Vertex, typename Edge>
void
GenericStateGraph<Vertex, Edge>::
AppendRoadmap(const GenericStateGraph& _r) {
  // Copy vertices and map the change in VIDs.
  std::unordered_map<VID, VID> oldToNew;
  for(auto vit = _r.begin(); vit != _r.end(); ++vit) {
    const VID oldVID = vit->descriptor();
    const VID newVID = AddVertex(vit->property());
    oldToNew[oldVID] = newVID;
  }
 
  // Copy edges.
  for(auto vit = _r.begin(); vit != _r.end(); ++vit) {
    for(auto eit = vit->begin(); eit != vit->end(); ++eit) {
      const VID source = oldToNew[eit->source()];
      const VID target = oldToNew[eit->target()];
      if(!IsEdge(source, target))
        AddEdge(source, target, eit->property());
    }
  }
}
 
 
template <typename Vertex, typename Edge>
inline
void
GenericStateGraph<Vertex, Edge>::
SetCCTracker(StatClass* const _stats) {
  m_ccTracker.reset(new CCTrackerType(this));
  if(_stats)
    m_ccTracker->SetStatClass(_stats);
}
 
/*-------------------------------- Queries -----------------------------------*/
 
template <typename Vertex, typename Edge>
inline
size_t
GenericStateGraph<Vertex, Edge>::
Size() const noexcept {
  return this->get_num_vertices();
}
 
 
template <typename Vertex, typename Edge>
inline
bool
GenericStateGraph<Vertex, Edge>::
IsVertex(const VID _vid) const noexcept {
  return this->find_vertex(_vid) != this->end();
}
 
template <typename Vertex, typename Edge>
inline
bool
GenericStateGraph<Vertex, Edge>::
IsVertex(const Vertex& _v) const noexcept {
  CVI vi;
  return IsVertex(_v, vi);
}
 
 
template <typename Vertex, typename Edge>
inline
bool
GenericStateGraph<Vertex, Edge>::
IsVertex(const Vertex& _v, CVI& _vi) const noexcept {
#ifndef _PARALLEL
  for(CVI vi = this->begin(); vi != this->end(); ++vi){
    if(vi->property() == _v){
      _vi = vi;
      return true;
    }
  }
#else
  std::cerr << "WARNING::STAPL working on fixing problem with const iterators\n";
#endif
  return false;
}
 
 
template <typename Vertex, typename Edge>
inline
bool
GenericStateGraph<Vertex, Edge>::
IsEdge(const VID _source, const VID _target) const noexcept {
  CEI ei;
  CVI vi;
  return this->find_edge(EID(_source, _target), vi, ei);
}
 
 
template <typename Vertex, typename Edge>
template <typename T>
inline
typename GenericStateGraph<Vertex, Edge>::VID
GenericStateGraph<Vertex, Edge>::
GetVID(const T& _t) const noexcept {
  return *_t;
}
 
 
template <typename Vertex, typename Edge>
inline
typename GenericStateGraph<Vertex, Edge>::VID
GenericStateGraph<Vertex, Edge>::
GetVID(const VI& _t) const noexcept {
  return _t->descriptor();
}
 
 
template <typename Vertex, typename Edge>
inline
typename GenericStateGraph<Vertex, Edge>::VID
GenericStateGraph<Vertex, Edge>::
GetVID(const Vertex& _t) const noexcept {
  CVI vi;
  if(IsVertex(_t, vi))
    return vi->descriptor();
  return INVALID_VID;
}
 
 
template <typename Vertex, typename Edge>
inline
const typename GenericStateGraph<Vertex, Edge>::VertexSet&
GenericStateGraph<Vertex, Edge>::
GetPredecessors(const VID _vid) const noexcept {
  return m_predecessors.at(_vid);
}
 
 
template <typename Vertex, typename Edge>
inline
typename GenericStateGraph<Vertex, Edge>::VID
GenericStateGraph<Vertex, Edge>::
GetLastVID() const noexcept {
  if(this->get_num_vertices() == 0)
    return INVALID_VID;
 
  return (--this->end())->descriptor();
}
 
 
template <typename Vertex, typename Edge>
inline
size_t
GenericStateGraph<Vertex, Edge>::
GetTimestamp() const noexcept {
  return m_timestamp;
}
 
 
template <typename Vertex, typename Edge>
inline
const typename GenericStateGraph<Vertex, Edge>::VertexSet&
GenericStateGraph<Vertex, Edge>::
GetAllVIDs() const noexcept {
  return m_allVIDs;
}
 
/*------------------------------- Accessors ----------------------------------*/
 
template <typename Vertex, typename Edge>
inline
Robot*
GenericStateGraph<Vertex, Edge>::
GetRobot() const noexcept {
  return m_robot;
}
 
 
template <typename Vertex, typename Edge>
inline
typename GenericStateGraph<Vertex, Edge>::CCTrackerType*
GenericStateGraph<Vertex, Edge>::
GetCCTracker() const noexcept {
  return m_ccTracker.get();
}
 
 
template <typename Vertex, typename Edge>
template <typename T>
inline
typename GenericStateGraph<Vertex, Edge>::VP&
GenericStateGraph<Vertex, Edge>::
GetVertex(T& _t) noexcept {
  return GetVertex(VID(*_t));
}
 
 
template <typename Vertex, typename Edge>
inline
typename GenericStateGraph<Vertex, Edge>::VP&
GenericStateGraph<Vertex, Edge>::
GetVertex(VI& _t) noexcept {
  return _t->property();
}
 
 
template <typename Vertex, typename Edge>
inline
typename GenericStateGraph<Vertex, Edge>::VP&
GenericStateGraph<Vertex, Edge>::
GetVertex(VID _t) noexcept {
  VI iter = this->find_vertex(_t);
  if(iter == this->end())
    throw RunTimeException(WHERE) << "Requested node '" << _t
                                  << "', which is not in the graph.";
 
  return iter->property();
}
 
 
template <typename Vertex, typename Edge>
template <typename T>
inline
const typename GenericStateGraph<Vertex, Edge>::VP&
GenericStateGraph<Vertex, Edge>::
GetVertex(T& _t) const noexcept {
  return GetVertex(VID(*_t));
}
 
 
template <typename Vertex, typename Edge>
inline
const typename GenericStateGraph<Vertex, Edge>::VP&
GenericStateGraph<Vertex, Edge>::
GetVertex(CVI& _t) const noexcept {
  return (*_t).property();
}
 
 
template <typename Vertex, typename Edge>
inline
const typename GenericStateGraph<Vertex, Edge>::VP&
GenericStateGraph<Vertex, Edge>::
GetVertex(VID _t) const noexcept {
  CVI iter = this->find_vertex(_t);
  if(iter == this->end())
    throw RunTimeException(WHERE) << "Requested node '" << _t
                                  << "', which is not in the graph.";
 
  return (*iter).property();
}
 
 
template <typename Vertex, typename Edge>
std::vector<typename GenericStateGraph<Vertex, Edge>::VID>
GenericStateGraph<Vertex, Edge>::
GetChildren(const VID _vid) const noexcept {
  auto vi = this->find_vertex(_vid);
  if(vi == this->end())
    throw RunTimeException(WHERE) << "Requested children of node '" << _vid
                                  << "', which is not in the graph.";
 
  std::vector<VID> output;
  for(const auto& e : *vi)
    output.push_back(e.target());
  return output;
}
 
 
template <typename Vertex, typename Edge>
size_t
GenericStateGraph<Vertex, Edge>::
GetInDegree(const VID _vid) noexcept {
  auto predIter = m_predecessors.find(_vid);
  VertexSet& preds = predIter->second;
  return preds.size();
}
 
 
template <typename Vertex, typename Edge>
std::vector<typename GenericStateGraph<Vertex, Edge>::EI>
GenericStateGraph<Vertex, Edge>::
FindInboundEdges(const VID _vid) {
  std::vector<EI> inEdges;
 
  auto predIter = m_predecessors.find(_vid);
  VertexSet& predecessors = predIter->second;
 
  for(const auto pred : predecessors) {
    auto predIter = this->find_vertex(pred);
    for(auto eit = predIter->begin(); eit != predIter->end(); ++eit)
      if(eit->target() == _vid)
        inEdges.push_back(eit);
  }
 
  return inEdges;
}
 
 
template <typename Vertex, typename Edge>
inline
bool
GenericStateGraph<Vertex, Edge>::
GetEdge(const VID _source, const VID _target, EI& _ei) noexcept {
  VI vi;
  return this->find_edge(EID(_source, _target), vi, _ei);
}
 
 
template <typename Vertex, typename Edge>
inline
bool
GenericStateGraph<Vertex, Edge>::
GetEdge(const VID _source, const VID _target, CEI& _ei) const noexcept {
  CVI vi;
  return this->find_edge(EID(_source, _target), vi, _ei);
}
 
 
template <typename Vertex, typename Edge>
inline
typename GenericStateGraph<Vertex, Edge>::EP&
GenericStateGraph<Vertex, Edge>::
GetEdge(const VID _source, const VID _target) noexcept {
  EI ei;
  if(!GetEdge(_source, _target, ei))
    throw RunTimeException(WHERE) << "Requested non-existent edge ("
                                  << _source << ", " << _target << ").";
 
  return ei->property();
}
 
 
template <typename Vertex, typename Edge>
inline
typename GenericStateGraph<Vertex, Edge>::EP&
GenericStateGraph<Vertex, Edge>::
GetEdge(const EID _descriptor) noexcept {
  return GetEdge(_descriptor.source(), _descriptor.target());
}
 
/*--------------------------------- Hooks ------------------------------------*/
 
template <typename Vertex, typename Edge>
bool
GenericStateGraph<Vertex, Edge>::
IsHook(const HookType _type, const std::string& _label) const {
  switch(_type) {
    case HookType::AddVertex:
      return m_addVertexHooks.count(_label);
    case HookType::DeleteVertex:
      return m_deleteVertexHooks.count(_label);
    case HookType::AddEdge:
      return m_addEdgeHooks.count(_label);
    case HookType::DeleteEdge:
      return m_deleteEdgeHooks.count(_label);
    default:
      throw RunTimeException(WHERE) << "Unrecognized hook type '"
                                    << ToString(_type) << "'.";
  }
}
 
 
template <typename Vertex, typename Edge>
void
GenericStateGraph<Vertex, Edge>::
InstallHook(const HookType _type, const std::string& _label,
    const VertexHook& _h) {
  // Ensure that the hook does not already exist.
  if(IsHook(_type, _label))
    throw RunTimeException(WHERE) << "Hook of type '" << ToString(_type)
                                  << "' with label '" << _label
                                  << "' already exists.";
 
  switch(_type) {
    case HookType::AddVertex:
      m_addVertexHooks[_label] = _h;
      break;
    case HookType::DeleteVertex:
      m_deleteVertexHooks[_label] = _h;
      break;
    case HookType::AddEdge:
    case HookType::DeleteEdge:
      throw RunTimeException(WHERE) << "Edge hook type '" << ToString(_type)
                                    << "' used with vertex hook function "
                                    << "labeled '" << _label << "'.";
    default:
      throw RunTimeException(WHERE) << "Unrecognized hook type '"
                                    << ToString(_type)
                                    << "' with label '" << _label << "'.";
  }
}
 
 
template <typename Vertex, typename Edge>
void
GenericStateGraph<Vertex, Edge>::
InstallHook(const HookType _type, const std::string& _label, const EdgeHook& _h) {
  // Ensure that the hook does not already exist.
  if(IsHook(_type, _label))
    throw RunTimeException(WHERE) << "Hook of type '" << ToString(_type)
                                  << "' with label '" << _label
                                  << "' already exists.";
 
  switch(_type) {
    case HookType::AddEdge:
      m_addEdgeHooks[_label] = _h;
      break;
    case HookType::DeleteEdge:
      m_deleteEdgeHooks[_label] = _h;
      break;
    case HookType::AddVertex:
    case HookType::DeleteVertex:
      throw RunTimeException(WHERE) << "Vertex hook type '" << ToString(_type)
                                    << "' used with edge hook function labeled '"
                                    << _label << "'.";
    default:
      throw RunTimeException(WHERE) << "Unrecognized hook type '"
                                    << ToString(_type)
                                    << "' with label '" << _label << "'.";
  }
}
 
 
template <typename Vertex, typename Edge>
void
GenericStateGraph<Vertex, Edge>::
RemoveHook(const HookType _type, const std::string& _label) {
  if(!IsHook(_type, _label))
    throw RunTimeException(WHERE) << "Hook of type '" << ToString(_type)
                                  << "' with label '" << _label
                                  << "' does not exist.";
 
  switch(_type) {
    case HookType::AddVertex:
      m_addVertexHooks.erase(_label);
      break;
    case HookType::DeleteVertex:
      m_deleteVertexHooks.erase(_label);
      break;
    case HookType::AddEdge:
      m_addEdgeHooks.erase(_label);
      break;
    case HookType::DeleteEdge:
      m_deleteEdgeHooks.erase(_label);
      break;
    default:
      throw RunTimeException(WHERE) << "Unrecognized hook type '"
                                    << ToString(_type)
                                    << "' with label '" << _label << "'.";
  }
}
 
 
template <typename Vertex, typename Edge>
inline
void
GenericStateGraph<Vertex, Edge>::
DisableHooks() noexcept {
  m_enableHooks = false;
}
 
 
template <typename Vertex, typename Edge>
inline
void
GenericStateGraph<Vertex, Edge>::
EnableHooks() noexcept {
  m_enableHooks = true;
}
 
 
template <typename Vertex, typename Edge>
void
GenericStateGraph<Vertex, Edge>::
ClearHooks() noexcept {
  m_addVertexHooks.clear();
  m_deleteVertexHooks.clear();
  m_addEdgeHooks.clear();
  m_deleteEdgeHooks.clear();
}
 
/*----------------------------------- I/O ------------------------------------*/
 
 
#if 1
template <typename GenericStateGraph>
void
Read(GenericStateGraph* _g, const std::string& _filename) {
  std::ifstream ifs(_filename);
  if(!ifs)
    throw ParseException(WHERE) << "Cannot open file '" << _filename << "'.";
 
  std::string tag;
  bool headerParsed = false;
  int graphStart = 0;
  // Read the file until we find the GRAPHSTART tag.
  while(!headerParsed) {
    // Mark our position and read the next line.
    graphStart = ifs.tellg();
    if(!(ifs >> tag))
      throw ParseException(WHERE) << "Error reading map file '" << _filename
                                  << "' - GRAPHSTART tag is missing.";
 
    // If we find the GRAPHSTART tag, we are done.
    if(tag.find("GRAPHSTART") != std::string::npos)
      headerParsed = true;
  }
 
  if(!_g->GetRobot())
    RunTimeException(WHERE) << "Must specify robot when reading in roadmaps.";
 
  typedef typename GenericStateGraph::STAPLGraph STAPLGraph;
  typedef typename GenericStateGraph::vertex_property Vertex;
  typedef typename GenericStateGraph::edge_property Edge;
 
  // Set the input robot for our edge class.
  Edge::inputRobot = _g->GetRobot();
  Vertex::inputRobot = _g->GetRobot();
 
  // Set ifs back to the line with the GRAPHSTART tag and read in the graph.
  ifs.seekg(graphStart, ifs.beg);
  stapl::sequential::read_graph<STAPLGraph>(*_g, ifs);
 
  // Unset the input robot for our edge class.
  Edge::inputRobot = nullptr;
  Vertex::inputRobot = nullptr;
 
  for(auto vi = _g->begin(); vi != _g->end(); ++vi)
    _g->ExecuteAddVertexHooks(vi);
  for(auto vi = _g->begin(); vi != _g->end(); ++vi)
    for(auto ei = vi->begin(); ei != vi->end(); ++ei)
      _g->ExecuteAddEdgeHooks(ei);
}
#else
template <typename Vertex, typename Edge>
void
GenericStateGraph<Vertex, Edge>::
Read(const std::string& _filename) {
  std::ifstream ifs(_filename);
  if(!ifs)
    throw ParseException(WHERE) << "Cannot open file '" << _filename << "'.";
 
  std::string tag;
  bool headerParsed = false;
  int graphStart = 0;
  // Read the file until we find the GRAPHSTART tag.
  while(!headerParsed) {
    // Mark our position and read the next line.
    graphStart = ifs.tellg();
    if(!(ifs >> tag))
      throw ParseException(WHERE) << "Error reading map file '" << _filename
                                  << "' - GRAPHSTART tag is missing.";
 
    // If we find the GRAPHSTART tag, we are done.
    if(tag.find("GRAPHSTART") != std::string::npos)
      headerParsed = true;
  }
 
  if(!GetRobot())
    RunTimeException(WHERE) << "Must specify robot when reading in roadmaps.";
 
  // Set the input robot for our edge class.
  Edge::inputRobot = GetRobot();
  Vertex::inputRobot = GetRobot();
 
  // Set ifs back to the line with the GRAPHSTART tag and read in the graph.
  ifs.seekg(graphStart, ifs.beg);
  stapl::sequential::read_graph<STAPLGraph>(*this, ifs);
 
  // Unset the input robot for our edge class.
  Edge::inputRobot = nullptr;
  Vertex::inputRobot = nullptr;
}
#endif
 
template <typename GenericStateGraph>
void
ReadMessage(GenericStateGraph* _g, const std::string& _msg) {
  std::stringstream ss(_msg);
 
  std::string tag;
  bool headerParsed = false;
  int graphStart = 0;
  // Read the file until we find the GRAPHSTART tag.
  while(!headerParsed) {
    // Mark our position and read the next line.
    graphStart = ss.tellg();
    if(!(ss >> tag))
      throw ParseException(WHERE) << "Error reading msg '" << _msg
                                  << "' - GRAPHSTART tag is missing.";
 
    // If we find the GRAPHSTART tag, we are done.
    if(tag.find("GRAPHSTART") != std::string::npos)
      headerParsed = true;
  }
 
  if(!_g->GetRobot())
    RunTimeException(WHERE) << "Must specify robot when reading in roadmaps.";
 
  typedef typename GenericStateGraph::STAPLGraph STAPLGraph;
  typedef typename GenericStateGraph::vertex_property Vertex;
  typedef typename GenericStateGraph::edge_property Edge;
 
  // Set the input robot for our edge class.
  Edge::inputRobot = _g->GetRobot();
  Vertex::inputRobot = _g->GetRobot();
 
  // Set ss back to the line with the GRAPHSTART tag and read in the graph.
  ss.seekg(graphStart, ss.beg);
  stapl::sequential::read_graph<STAPLGraph>(*_g, ss);
 
  // Unset the input robot for our edge class.
  Edge::inputRobot = nullptr;
  Vertex::inputRobot = nullptr;
 
  for(auto vi = _g->begin(); vi != _g->end(); ++vi)
    _g->ExecuteAddVertexHooks(vi);
  for(auto vi = _g->begin(); vi != _g->end(); ++vi)
    for(auto ei = vi->begin(); ei != vi->end(); ++ei)
      _g->ExecuteAddEdgeHooks(ei);
}
 
template <typename Vertex, typename Edge>
void
GenericStateGraph<Vertex, Edge>::
Write(const std::string& _filename, Environment* _env) const {
  std::ofstream ofs(_filename);
  ofs << "#####ENVFILESTART#####" << std::endl
      << _env->GetEnvFileName() << std::endl
      << "#####ENVFILESTOP#####" << std::endl;
 
#ifndef _PARALLEL
  stapl::sequential::write_graph(*this, ofs);
#else
  ofs.close();
  stapl::graph_view<STAPLGraph> gv(*this);
  write_PMPL_graph(gv, _filename);
#endif
 
}
 
/*-------------------------------- Helpers -----------------------------------*/
 
template <typename Vertex, typename Edge>
inline
void
GenericStateGraph<Vertex, Edge>::
ExecuteAddVertexHooks(const VI _iterator) noexcept {
  if(!m_enableHooks)
    return;
 
  if(m_ccTracker)
    m_ccTracker->AddVertex(_iterator);
 
  for(auto& hook : m_addVertexHooks)
    hook.second(_iterator);
}
 
 
template <typename Vertex, typename Edge>
inline
void
GenericStateGraph<Vertex, Edge>::
ExecuteDeleteVertexHooks(const VI _iterator) noexcept {
  if(!m_enableHooks)
    return;
 
  for(auto& hook : m_deleteVertexHooks)
    hook.second(_iterator);
 
  if(m_ccTracker)
    m_ccTracker->DeleteVertex(_iterator);
}
 
 
template <typename Vertex, typename Edge>
inline
void
GenericStateGraph<Vertex, Edge>::
ExecuteAddEdgeHooks(const EI _iterator) noexcept {
  if(!m_enableHooks)
    return;
 
  if(m_ccTracker)
    m_ccTracker->AddEdge(_iterator);
 
  for(auto& hook : m_addEdgeHooks)
    hook.second(_iterator);
}
 
 
template <typename Vertex, typename Edge>
inline
void
GenericStateGraph<Vertex, Edge>::
ExecuteDeleteEdgeHooks(const EI _iterator) noexcept {
  if(!m_enableHooks)
    return;
 
  for(auto& hook : m_deleteEdgeHooks)
    hook.second(_iterator);
 
  if(m_ccTracker)
    m_ccTracker->DeleteEdge(_iterator);
}
 
 
template <typename Vertex, typename Edge>
std::string
GenericStateGraph<Vertex, Edge>::
ToString(const HookType& _t) const noexcept {
  switch(_t) {
    case HookType::AddVertex:
      return "AddVertex";
    case HookType::DeleteVertex:
      return "DeleteVertex";
    case HookType::AddEdge:
      return "AddEdge";
    case HookType::DeleteEdge:
      return "DeleteEdge";
    default:
      return std::string(1, char(_t));
  }
}
 
/*----------------------------------------------------------------------------*/
 
inline
std::unordered_set<size_t>
VertexSetIntersection(const std::unordered_set<size_t>& _a,
    const std::unordered_set<size_t>& _b) {
  using VertexSet = std::unordered_set<size_t>;
 
  // Find the smaller of the two sets.
  const VertexSet* smaller{nullptr},
                 * larger{nullptr};
  if(_a.size() < _b.size()) {
    smaller = &_a;
    larger  = &_b;
  }
  else {
    smaller = &_b;
    larger  = &_a;
  }
 
  // Test each element in the smaller set for membership in the larger one.
  VertexSet intersection;
  std::copy_if(smaller->begin(), smaller->end(),
               std::inserter(intersection, intersection.end()),
               [larger](const size_t _vid) {return larger->count(_vid);});
  return intersection;
}
 
 
inline
bool
HaveCommonVertex(const std::unordered_set<size_t>& _a,
                 const std::unordered_set<size_t>& _b) {
  using VertexSet = std::unordered_set<size_t>;
 
  // Find the smaller of the two sets.
  const VertexSet* smaller,
                 * larger;
  if(_a.size() < _b.size()) {
    smaller = &_a;
    larger  = &_b;
  }
  else {
    smaller = &_b;
    larger  = &_a;
  }
 
  // Test each element in the smaller set for membership in the larger one.
  return std::any_of(smaller->begin(), smaller->end(),
                     [larger](const size_t _vid) {return larger->count(_vid);});
}
 
inline
std::unordered_set<size_t>&
VertexSetUnionInPlace(std::unordered_set<size_t>& _receiver,
    const std::unordered_set<size_t>& _source) {
  // Reserve sufficient space for the worst case. This wastes a bit of space
  // (which is inevitable anyway) but avoids more than one rehash.
  _receiver.reserve(_receiver.size() + _source.size());
  for(const size_t vid : _source)
    _receiver.insert(vid);
  return _receiver;
}
 
template <typename T, typename... Rest>
inline
const T&
RandomElement(const std::unordered_set<T, Rest...>& _set) noexcept {
  if(_set.empty())
    throw RunTimeException(WHERE) << "Can't select a random element from an "
                                  << "empty set.";
#if 1
  // Pick a random bucket within the set.
  size_t bucketIndex, bucketSize, tries = 0;
  do {
    ++tries;
    bucketIndex = DRand() * _set.bucket_count();
    bucketSize  = _set.bucket_size(bucketIndex);
  }
  while(bucketSize == 0);
 
  const size_t elementIndex = DRand() * bucketSize;
#if 0
  std::cout << "\nset size = " << _set.size()
            << "\ntries = " << tries
            << "\nbucketIndex = "  << bucketIndex
            << "\nbucketSize  = "  << bucketSize
            << "\nelementIndex = " << elementIndex
            << "\nelement = "
            << *std::next(_set.begin(bucketIndex), elementIndex)
            << std::endl;
#endif
  return *std::next(_set.begin(bucketIndex), elementIndex);
#else
  // This is the fully linear solution, which works better for very small sets.
  // It is retained here for reference.
  return *std::next(_set.begin(), DRand() * _set.size());
#endif
}
 
/*----------------------------------------------------------------------------*/
 
#endif