// BFS.hpp
//
//     Created on: 13. 11. 2017
//         Author: Jan Uhlik
//    Modified by:
//
// Copyright (c) 2017 Czech Technical University in Prague | Faculty of Information Technology. All rights reserved.
// Git repository: https://gitlab.fit.cvut.cz/algorithms-library-toolkit/automata-library

#ifndef ALIB2_BFS_HPP
#define ALIB2_BFS_HPP

#include <functional>
#include <queue>
#include <alib/map>
#include <alib/set>
#include <iostream>

#include <common/ReconstructPath.hpp>

namespace graph {

namespace traverse {

class BFS {
// ---------------------------------------------------------------------------------------------------------------------

 public:

  /// Run BFS algorithm from the \p start node in the \p graph.
  ///
  /// Whenever node is opened, \p f_user is called with two parameters (the opened node and distance to this node).
  /// If return of \p f_user is true, then the algorithm is stopped.
  ///
  /// \param graph to explore.
  /// \param start initial node.
  /// \param f_user function which is called for every opened node with value of currently shortest path.
  ///
  template<
      typename TGraph,
      typename TNode,
      typename F = std::function<void(const TNode &, const size_t &)>>
  static
  void
  run(const TGraph &graph,
      const TNode &start,
      F f_user = [](const TNode &, const size_t &) -> bool { return false; });

// ---------------------------------------------------------------------------------------------------------------------

  /// Find the shortest path using BFS algorithm from the \p start node to the \p goal node in the \p graph.
  ///
  /// Whenever node is opened, \p f_user is called with two parameters (the opened node and distance to this node).
  ///
  /// \param graph to explore.
  /// \param start initial node.
  /// \param goal final node.
  /// \param f_user function which is called for every opened node with value of currently shortest path.
  ///
  /// \returns nodes in shortest path, if there is no such path vector is empty.
  ///
  template<
      typename TGraph,
      typename TNode,
      typename F = std::function<void(const TNode &, const size_t &)>>
  static
  ext::vector<TNode>
  findPath(const TGraph &graph,
           const TNode &start,
           const TNode &goal,
           F f_user = [](const TNode &, const size_t &) {});

  template<typename TGraph, typename TNode>
  static
  ext::vector<TNode>
  findPathRegistration(const TGraph &graph,
                       const TNode &start,
                       const TNode &goal) {
    return findPath(graph, start, goal);

  }

// ---------------------------------------------------------------------------------------------------------------------

  /// Find the shortest path using BFS algorithm from the \p start node to the \p goal node in the \p graph.
  /// This algorithm is run in both direction, from \p start and also from \p goal.
  ///
  /// Whenever node is opened, \p f_user is called with two parameters (the opened node and distance to this node).
  ///
  /// \param graph to explore.
  /// \param start initial node.
  /// \param goal final node.
  /// \param f_user function which is called for every opened node with value of currently shortest path.
  ///
  /// \returns nodes in shortest path, if there is no such path vector is empty.
  ///
  template<
      typename TGraph,
      typename TNode,
      typename F = std::function<void(const TNode &, const size_t &)>>
  static
  ext::vector<TNode>
  findPathBidirectional(const TGraph &graph,
                        const TNode &start,
                        const TNode &goal,
                        F f_user = [](const TNode &, const size_t &) {});

  template<typename TGraph, typename TNode>
  static
  ext::vector<TNode>
  findPathBidirectionalRegistration(const TGraph &graph,
                                    const TNode &start,
                                    const TNode &goal) {
    return findPath(graph, start, goal);
  }

// =====================================================================================================================

 private:

// ---------------------------------------------------------------------------------------------------------------------

  template<typename TNode>
  struct Data {
    std::queue<TNode> queue;
    ext::set<TNode> v;
    ext::map<TNode, TNode> p;
    ext::map<TNode, size_t> d;
  };

// ---------------------------------------------------------------------------------------------------------------------

  template<typename TGraph, typename TNode, typename F1, typename F2>
  static
  ext::vector<TNode>
  implNormal(const TGraph &graph,
             const TNode &start,
             const TNode &goal,
             F1 f_user,
             F2 f_stop);

// ---------------------------------------------------------------------------------------------------------------------

  template<typename FSucc, typename TNode, typename F1, typename F2>
  static bool expansion(FSucc successors,
                        Data<TNode> &data,
                        F1 f_user,
                        F2 f_stop);

// ---------------------------------------------------------------------------------------------------------------------

  template<typename TGraph, typename TNode, typename F1>
  static
  ext::vector<TNode>
  implBidirectional(const TGraph &graph,
                    const TNode &start,
                    const TNode &goal,
                    F1 f_user);

// ---------------------------------------------------------------------------------------------------------------------

  template<typename TNode>
  inline static void init(BFS::Data<TNode> &data, const TNode &start);

// ---------------------------------------------------------------------------------------------------------------------
};

// =====================================================================================================================

template<typename TGraph, typename TNode, typename F>
void BFS::run(const TGraph &graph, const TNode &start, F f_user) {
  // goal -> start: in order to avoid call constructor
  implNormal(graph, start, start, f_user, [](const TNode &) -> bool { return false; });
}

// ---------------------------------------------------------------------------------------------------------------------

template<typename TGraph, typename TNode, typename F>
ext::vector<TNode>
BFS::findPath(const TGraph &graph,
              const TNode &start,
              const TNode &goal,
              F f_user) {
  return implNormal(graph, start, goal,
                    [&](const TNode &n, const size_t &d) -> bool {
                      f_user(n, d);
                      return false;
                    },
                    [&goal](const TNode &n) -> bool { return goal == n; });
}

// ---------------------------------------------------------------------------------------------------------------------

template<typename TGraph, typename TNode, typename F>
ext::vector<TNode>
BFS::findPathBidirectional(const TGraph &graph,
                           const TNode &start,
                           const TNode &goal,
                           F f_user) {
  return implBidirectional(graph, start, goal,
                           [&](const TNode &n, const size_t &d) -> bool {
                             f_user(n, d);
                             return false;
                           });
}

// =====================================================================================================================

// ---------------------------------------------------------------------------------------------------------------------

template<typename TGraph, typename TNode, typename F1, typename F2>
ext::vector<TNode>
BFS::implNormal(const TGraph &graph,
                const TNode &start,
                const TNode &goal,
                F1 f_user,
                F2 f_stop) {
  Data<TNode> data;

  // Init search
  init(data, start);

  while (!data.queue.empty()) {
    bool stop = expansion([&](const TNode &node) { return graph.successors(node); }, data, f_user, f_stop);

    if (stop) {
      break;
    }

  }

  return common::ReconstructPath::reconstructPath(data.p, start, goal);
}

// ---------------------------------------------------------------------------------------------------------------------

template<typename FSucc, typename TNode, typename F1, typename F2>
bool BFS::expansion(FSucc successors,
                    Data<TNode> &data,
                    F1 f_user,
                    F2 f_stop) {
  TNode n = data.queue.front();
  data.queue.pop();

  // Run user's function
  if (f_user(n, data.d[n])) {
    return true;
  }

  // Stop if reach the goal
  if (f_stop(n)) {
    return true;
  }

  for (const auto &s : successors(n)) {
    if (data.v.count(s) == 0) {
      data.v.insert(s);
      data.p.insert_or_assign(s, n);
      data.queue.push(s);
    }
  }

  return false;
}

// ---------------------------------------------------------------------------------------------------------------------

template<typename TGraph, typename TNode, typename F1>
ext::vector<TNode>
BFS::implBidirectional(const TGraph &graph,
                       const TNode &start,
                       const TNode &goal,
                       F1 f_user) {
  Data<TNode> forward_data;
  Data<TNode> backward_data;
  ext::vector<TNode> intersection;

  // Init forward search
  init(forward_data, start);

  // Init backward search
  init(backward_data, goal);

  while (!forward_data.queue.empty() && !backward_data.queue.empty()) {
    // Forward search relaxation
    bool stop = expansion([&](const TNode &node) { return graph.successors(node); }, forward_data, f_user,
                          [&](const TNode &n) {
                            if (backward_data.v.find(n) != backward_data.v.end()) {
                              intersection.push_back(n);
                              return true;
                            }
                            return false;
                          });


    // If there is a intersection, then we can reconstruct path
    if (stop) {
      return common::ReconstructPath::reconstructPath(forward_data.p,
                                                      backward_data.p,
                                                      start,
                                                      goal,
                                                      *intersection.begin());
    }

    // Backward search relaxation
    stop = expansion([&](const TNode &node) { return graph.predecessors(node); }, backward_data, f_user,
                     [&](const TNode &n) {
                       if (forward_data.v.find(n) != forward_data.v.end()) {
                         intersection.push_back(n);
                         return true;
                       }
                       return false;
                     });

    // If there is a intersection, then we can reconstruct path
    if (stop) {
      return common::ReconstructPath::reconstructPath(forward_data.p,
                                                      backward_data.p,
                                                      start,
                                                      goal,
                                                      *intersection.begin());
    }
  }

  // Path not found -> return empty vector
  return ext::vector<TNode>();
}

// ---------------------------------------------------------------------------------------------------------------------

template<typename TNode>
void BFS::init(BFS::Data<TNode> &data, const TNode &start) {
  data.queue.push(start);
  data.v.insert(start);
  data.p.insert_or_assign(start, start);
  data.d[start] = 0;
}

// ---------------------------------------------------------------------------------------------------------------------

} // namespace traverse

} // namespace graph

#endif // ALIB2_BFS_HPP
