/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */
#pragma once

#include <mrpt/core/aligned_allocator.h>
#include <mrpt/core/exceptions.h>
#include <mrpt/graphs/TNodeID.h>
#include <mrpt/typemeta/TTypeName.h>

#include <fstream>
#include <map>
#include <set>

namespace mrpt
{
namespace graphs
{
/** \addtogroup mrpt_graphs_grp
  @{ */

/** Used in mrpt::graphs export functions to .dot files \sa
 * mrpt::graphs::CDirectedGraph::saveAsDot */
struct TGraphvizExportParams
{
  /** If true (default=false), an "[constraint=false]" will be added to all
   * edges (see Graphviz docs). */
  bool mark_edges_as_not_constraint{false};
  /** If provided, these textual names will be used for naming the nodes
   * instead of their numeric IDs given in the edges. */
  std::map<TNodeID, std::string> node_names;
  /** If provided, an extra line will be added setting Graphviz properties for
   * each node, e.g. to set a node position, use the string "pos = \"x,y\"" */
  std::map<TNodeID, std::string> node_props;

  TGraphvizExportParams() = default;
};

namespace detail
{
/** An empty structure */
struct edge_annotations_empty
{
  DECLARE_TTYPENAME_CLASSNAME(mrpt::graphs::detail::edge_annotations_empty)
};
}  // namespace detail

/** A directed graph with the argument of the template specifying the type of
 * the annotations in the edges.
 *  This class only keeps a list of edges (in the member \a edges), so there is
 * no information stored for each node but its existence referred by a node_ID.
 *
 *  Note that edges are stored as a std::multimap<> to allow <b>multiple
 * edges</b> between the same pair of nodes.
 *
 * \sa mrpt::graphs::CDijkstra, mrpt::graphs::CNetworkOfPoses,
 * mrpt::graphs::CDirectedTree
 * \ingroup mrpt_graphs_grp
 */
template <class TYPE_EDGES, class EDGE_ANNOTATIONS = detail::edge_annotations_empty>
class CDirectedGraph
{
 public:
  /** The type of each global pose in \a nodes: an extension of the \a
   * TYPE_EDGES pose with any optional user-defined data */
  struct edge_t : public TYPE_EDGES, public EDGE_ANNOTATIONS
  {
    // Replicate possible constructors:
    inline edge_t() : TYPE_EDGES() {}
    template <typename... Args>
    inline edge_t(Args&&... a) : TYPE_EDGES(std::forward<Args>(a)...)
    {
    }
    constexpr static auto getClassName()
    {
      using namespace mrpt::typemeta;
      return literal("edge_t<") + TTypeName<TYPE_EDGES>::get() + literal(",") +
             TTypeName<EDGE_ANNOTATIONS>::get() + literal(">");
    }
  };
  /** Underlying type for edge_t = TYPE_EDGES + annotations */
  using edge_underlying_t = TYPE_EDGES;
  /** The type of the member \a edges */
  using edges_map_t = std::multimap<TPairNodeIDs, edge_t>;
  using iterator = typename edges_map_t::iterator;
  using reverse_iterator = typename edges_map_t::reverse_iterator;
  using const_iterator = typename edges_map_t::const_iterator;
  using const_reverse_iterator = typename edges_map_t::const_reverse_iterator;
  /**\brief Handy self type */
  using self_t = CDirectedGraph<TYPE_EDGES, EDGE_ANNOTATIONS>;

  /** The public member with the directed edges in the graph */
  edges_map_t edges;

  /** Copy constructor from a multimap<pair< >, > */
  inline CDirectedGraph(const edges_map_t& obj) : edges(obj) {}
  /** Default constructor */
  inline CDirectedGraph() : edges() {}
  inline iterator begin() { return edges.begin(); }
  inline iterator rbegin() { return edges.rbegin(); }
  inline iterator end() { return edges.end(); }
  inline iterator rend() { return edges.rend(); }
  inline const_iterator begin() const { return edges.begin(); }
  inline const_iterator rbegin() const { return edges.rbegin(); }
  inline const_iterator end() const { return edges.end(); }
  inline const_iterator rend() const { return edges.rend(); }
  /** @name Edges/nodes utility methods
    @{ */

  /** The number of edges in the graph */
  inline size_t edgeCount() const { return edges.size(); }
  /** Erase all edges */
  inline void clearEdges() { edges.clear(); }
  /** Insert an edge (from -> to) with the given edge value. \sa
   * insertEdgeAtEnd */
  inline void insertEdge(TNodeID from_nodeID, TNodeID to_nodeID, const edge_t& edge_value)
  {
    alignas(MRPT_MAX_STATIC_ALIGN_BYTES)
        typename edges_map_t::value_type entry(std::make_pair(from_nodeID, to_nodeID), edge_value);
    edges.insert(entry);
  }

  /** Insert an edge (from -> to) with the given edge value (more efficient
   * version to be called if you know that the end will go at the end of the
   * sorted std::multimap). \sa insertEdge */
  inline void insertEdgeAtEnd(TNodeID from_nodeID, TNodeID to_nodeID, const edge_t& edge_value)
  {
    alignas(MRPT_MAX_STATIC_ALIGN_BYTES)
        typename edges_map_t::value_type entry(std::make_pair(from_nodeID, to_nodeID), edge_value);
    edges.insert(edges.end(), entry);
  }

  /** Test if the given directed edge exists. */
  inline bool edgeExists(TNodeID from_nodeID, TNodeID to_nodeID) const
  {
    return edges.find(std::make_pair(from_nodeID, to_nodeID)) != edges.end();
  }

  /** Return a reference to the content of a given edge.
   *  If several edges exist between the given nodes, the first one is
   * returned.
   * \exception std::exception if the given edge does not exist
   * \sa getEdges
   */
  edge_t& getEdge(TNodeID from_nodeID, TNodeID to_nodeID)
  {
    iterator it = edges.find(std::make_pair(from_nodeID, to_nodeID));
    if (it == edges.end())
    {
      THROW_EXCEPTION_FMT("Edge %u->%u does not exist", (unsigned)from_nodeID, (unsigned)to_nodeID);
    }
    else
      return it->second;
  }

  /** Return a reference to the content of a given edge.
   *  If several edges exist between the given nodes, the first one is
   * returned.
   * \exception std::exception if the given edge does not exist
   * \sa getEdges
   */
  const edge_t& getEdge(TNodeID from_nodeID, TNodeID to_nodeID) const
  {
    const_iterator it = edges.find(std::make_pair(from_nodeID, to_nodeID));
    if (it == edges.end())
    {
      THROW_EXCEPTION_FMT("Edge %u->%u does not exist", (unsigned)from_nodeID, (unsigned)to_nodeID);
    }
    else
      return it->second;
  }

  /** Return a pair<first,last> of iterators to the range of edges between two
   * given nodes. \sa getEdge  */
  std::pair<iterator, iterator> getEdges(TNodeID from_nodeID, TNodeID to_nodeID)
  {
    return edges.equal_range(std::make_pair(from_nodeID, to_nodeID));
  }
  /** Return a pair<first,last> of const iterators to the range of edges
   * between two given nodes.  \sa getEdge */
  std::pair<const_iterator, const_iterator> getEdges(TNodeID from_nodeID, TNodeID to_nodeID) const
  {
    return edges.equal_range(std::make_pair(from_nodeID, to_nodeID));
  }

  /** Erase all edges between the given nodes (it has no effect if no edge
   * existed)
   */
  inline void eraseEdge(TNodeID from_nodeID, TNodeID to_nodeID)
  {
    edges.erase(std::make_pair(from_nodeID, to_nodeID));
  }

  /** Return a list of all the node_ID's of the graph, generated from all the
   * nodes that appear in the list of edges
   */
  void getAllNodes(std::set<TNodeID>& lstNode_IDs) const
  {
    lstNode_IDs.clear();
    for (auto it = edges.begin(); it != edges.end(); ++it)
    {
      lstNode_IDs.insert(it->first.first);
      lstNode_IDs.insert(it->first.second);
    }
  }

  /** Less efficient way to get all nodes that returns a copy of the set
   * object \sa getAllNodes( std::set<TNodeID> &lstNode_IDs) */
  inline std::set<TNodeID> getAllNodes() const
  {
    std::set<TNodeID> lst;
    getAllNodes(lst);
    return lst;
  }

  /** Count how many different node IDs appear in the graph edges.
   */
  size_t countDifferentNodesInEdges() const
  {
    std::set<TNodeID> aux;
    for (typename edges_map_t::const_iterator it = edges.begin(); it != edges.end(); ++it)
    {
      aux.insert(it->first.first);
      aux.insert(it->first.second);
    }
    return aux.size();
  }

  /** Return the list of all neighbors of "nodeID", by creating a list of
   * their node IDs. \sa getAdjacencyMatrix */
  void getNeighborsOf(TNodeID const nodeID, std::set<TNodeID>& neighborIDs) const
  {
    neighborIDs.clear();
    for (typename edges_map_t::const_iterator it = edges.begin(); it != edges.end(); ++it)
    {
      if (it->first.first == nodeID)
        neighborIDs.insert(it->first.second);
      else if (it->first.second == nodeID)
        neighborIDs.insert(it->first.first);
    }
  }
  /** Return the list of all neighbors of "nodeID", by creating a list of
   * their node IDs. \sa getAdjacencyMatrix */
  std::set<TNodeID> getNeighborsOf(TNodeID const nodeID) const
  {
    std::set<TNodeID> neighborIDs;
    this->getNeighborsOf(nodeID, neighborIDs);
    return neighborIDs;
  }

  /** Return a map from node IDs to all its neighbors (that is, connected
   * nodes, regardless of the edge direction)
   *  This is a much more efficient method than calling getNeighborsOf() for
   * each node in the graph.
   *  Possible values for the template argument MAP_NODEID_SET_NODEIDS are:
   *    - std::map<TNodeID, std::set<TNodeID> >
   *    - mrpt::containers::map_as_vector<TNodeID, std::set<TNodeID> >
   * \sa getNeighborsOf
   */
  template <class MAP_NODEID_SET_NODEIDS>
  void getAdjacencyMatrix(MAP_NODEID_SET_NODEIDS& outAdjacency) const
  {
    outAdjacency.clear();
    for (auto it = edges.begin(); it != edges.end(); ++it)
    {
      outAdjacency[it->first.first].insert(it->first.second);
      outAdjacency[it->first.second].insert(it->first.first);
    }
  }

  /** Just like \a getAdjacencyMatrix but return only the adjacency for those
   * node_ids in the set \a onlyForTheseNodes
   *  (both endings nodes of an edge must be within the set for it to be
   * returned) */
  template <class MAP_NODEID_SET_NODEIDS, class SET_NODEIDS>
  void getAdjacencyMatrix(
      MAP_NODEID_SET_NODEIDS& outAdjacency, const SET_NODEIDS& onlyForTheseNodes) const
  {
    outAdjacency.clear();
    const typename SET_NODEIDS::const_iterator setEnd = onlyForTheseNodes.end();
    for (typename edges_map_t::const_iterator it = edges.begin(); it != edges.end(); ++it)
    {
      if (onlyForTheseNodes.find(it->first.first) == setEnd ||
          onlyForTheseNodes.find(it->first.second) == setEnd)
        continue;
      outAdjacency[it->first.first].insert(it->first.second);
      outAdjacency[it->first.second].insert(it->first.first);
    }
  }

  /** @} */  // end of edge/nodes utilities

  /** @name I/O utilities
    @{ */

  /** Save the graph in a Graphviz (.dot files) text format; useful for
   * quickly rendering the graph with "dot"
   * \return false on any error */
  [[nodiscard]] bool saveAsDot(
      std::ostream& o, const TGraphvizExportParams& p = TGraphvizExportParams()) const
  {
    o << "digraph G {\n";
    for (const_iterator it = begin(); it != end(); ++it)
    {
      const TNodeID id1 = it->first.first;
      const TNodeID id2 = it->first.second;
      std::string s1, s2;
      if (!p.node_names.empty())
      {
        auto itNam1 = p.node_names.find(id1);
        if (itNam1 != p.node_names.end()) s1 = itNam1->second;
        auto itNam2 = p.node_names.find(id2);
        if (itNam2 != p.node_names.end()) s2 = itNam2->second;
      }
      if (s1.empty()) s1 = std::to_string(id1);
      if (s2.empty()) s2 = std::to_string(id2);
      if (p.node_props.empty())
      {
        auto itP1 = p.node_props.find(id1);
        if (itP1 != p.node_props.end())
          o << "\"" << s1 << "\""
            << " [" << itP1->second << "];\n";
        auto itP2 = p.node_props.find(id2);
        if (itP2 != p.node_props.end())
          o << "\"" << s2 << "\""
            << " [" << itP2->second << "];\n";
      }
      o << " \"" << s1 << "\" -> \"" << s2 << "\"";
      if (p.mark_edges_as_not_constraint) o << " [constraint=false]";
      o << ";\n";
    }
    return !((o << "}\n").fail());
  }

  /** \overload */
  bool saveAsDot(
      const std::string& fileName, const TGraphvizExportParams& p = TGraphvizExportParams()) const
  {
    std::ofstream f(fileName.c_str());
    if (!f.is_open()) return false;
    return saveAsDot(f, p);
  }
  /** @} */

};  // end class CDirectedGraph

/** @} */
}  // namespace graphs
}  // namespace mrpt
