// IDIDDFS.hpp
//
//     Created on: 27. 02. 2018
//         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

#pragma once

#include <functional>
#include <alib/vector>
#include <queue>
#include <limits>
#include <alib/set>
#include <alib/map>

#include <common/ReconstructPath.hpp>

namespace graph {

namespace traverse {

class IDDFS {
// ---------------------------------------------------------------------------------------------------------------------

 public:

  /// Run IDDFS algorithm from the \p start node in the \p graph.
  /// Be aware that running this algorithm on a non-tree graph gonna be extremely slow.
  ///
  /// 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 max_depth set maximal allowed depth for IDDFS.
  /// \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<bool(const TNode &, const size_t &)>>
  static
  void
  run(const TGraph &graph,
      const TNode &start,
      unsigned long max_depth = std::numeric_limits<unsigned long>::max(),
      F f_user = [](const TNode &, const size_t &) -> bool { return false; });

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

  /// Find the shortest path using IDDFS algorithm from the \p start node to the \p goal node in the \p graph.
  /// Be aware that running this algorithm on a non-tree graph gonna be extremely slow.
  ///
  /// 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 max_depth set maximal allowed depth for IDDFS.
  /// \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,
           unsigned long max_depth = std::numeric_limits<unsigned long>::max(),
           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 IDDFS 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.
  /// Be aware that running this algorithm on a non-tree graph gonna be extremely slow.
  ///
  /// 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 max_depth set maximal allowed depth for IDDFS.
  /// \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,
                        unsigned long max_depth = std::numeric_limits<unsigned long>::max(),
                        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 {
    ext::vector<TNode> path; // current path
    ext::set<TNode> path_set; // current path set for quick search
    ext::set<TNode> v; // visited
  };

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

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

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

  template<typename FSucc, typename TNode, typename F1, typename F2>
  static
  bool
  expansion(FSucc successors,
            IDDFS::Data<TNode> &data,
            const TNode &n,
            bool save_nodes,
            unsigned long max_depth,
            F1 f_user,
            F2 f_stop);

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

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

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

  template<typename FSucc, typename TNode, typename F1, typename F2>
  static
  ext::set<TNode>
  backward_expansion(FSucc successors,
                     const IDDFS::Data<TNode> &forward_data,
                     IDDFS::Data<TNode> &backward_data,
                     const TNode &node,
                     unsigned long max_depth,
                     F1 f_user,
                     F2 f_stop);

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

  template<typename TGraph, typename TNode>
  static
  ext::vector<TNode>
  constructPath(const TGraph &graph,
                const TNode &start,
                const TNode &goal,
                TNode intersection,
                unsigned long max_forward_depth,
                unsigned long max_backward_depth);

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

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

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

};

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

template<typename TGraph, typename TNode, typename F>
void IDDFS::run(const TGraph &graph, const TNode &start, unsigned long max_depth, F f_user) {
  implNormal(graph, start, max_depth, f_user, [](const TNode &) -> bool { return false; });
}

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

template<typename TGraph, typename TNode, typename F>
ext::vector<TNode> IDDFS::findPath(const TGraph &graph,
                                   const TNode &start,
                                   const TNode &goal,
                                   unsigned long max_depth,
                                   F f_user) {
  return implNormal(graph, start, max_depth,
                    [&](const TNode &n, const auto &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> IDDFS::findPathBidirectional(const TGraph &graph,
                                                const TNode &start,
                                                const TNode &goal,
                                                unsigned long max_depth,
                                                F f_user) {
  return implBidirectional(graph, start, goal, max_depth,
                           [&](const TNode &n, const auto &d) -> bool {
                             f_user(n, d);
                             return false;
                           },
                           [](const TNode &) -> bool { return false; });
}

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

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

template<typename TGraph, typename TNode, typename F1, typename F2>
ext::vector<TNode> IDDFS::implNormal(const TGraph &graph,
                                     const TNode &start,
                                     unsigned long max_depth,
                                     F1 f_user,
                                     F2 f_stop) {
  for (unsigned long i = 1; i < max_depth; ++i) {
    Data<TNode> data;

    // Init search
    init(data, start);

    bool stop =
        expansion([&](const TNode &node) { return graph.successors(node); }, data, start, false, i, f_user, f_stop);

    if (stop) {
      return data.path;
    }
  }

  return ext::vector<TNode>();
}

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

template<typename FSucc, typename TNode, typename F1, typename F2>
bool IDDFS::expansion(FSucc successors,
                      IDDFS::Data<TNode> &data,
                      const TNode &n,
                      bool save_nodes,
                      unsigned long max_depth,
                      F1 f_user,
                      F2 f_stop) {
  if (max_depth == 0) return false; // Continue with search
  else if (save_nodes && max_depth == 1) {
    data.v.insert(n); // Remember only the deepest nodes
  }

  // Run user's function
  if (f_user(n, data.path.size())) {
    return true; // Stop algorithm
  }

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

  for (const auto &s : successors(n)) {

    // Check if node is not in path
    if (data.path_set.find(s) != data.path_set.end()) {
      continue;
    }

    data.path.push_back(s); // Insert current node
    data.path_set.insert(s);
    bool stop = expansion(successors, data, s, save_nodes, max_depth - 1, f_user, f_stop);

    if (stop) {
      return true; // Stop algorithm
    }

    data.path.pop_back(); // Remove current node
    data.path_set.erase(data.path_set.find(s));
  }

  return false; // Continue with search
}

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

template<typename TGraph, typename TNode, typename F1, typename F2>
ext::vector<TNode> IDDFS::implBidirectional(const TGraph &graph,
                                            const TNode &start,
                                            const TNode &goal,
                                            unsigned long max_depth,
                                            F1 f_user,
                                            F2 f_stop) {
  for (unsigned long i = 1; i < max_depth; ++i) {
    // -------------------------------
    // Forward search
    Data<TNode> forward_data;

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

    expansion([&](const TNode &n) { return graph.successors(n); }, forward_data, start, true, i, f_user, f_stop);

    // -------------------------------
    // Backward search

    ext::set<TNode> intersection;

    Data<TNode> backward_data;

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

    intersection = backward_expansion([&](const TNode &node) { return graph.predecessors(node); },
                                      forward_data,
                                      backward_data,
                                      goal,
                                      i,
                                      f_user,
                                      f_stop); // even run

    // If there is a intersection, then we can construct path
    if (intersection.size() != 0) {
      return constructPath(graph, start, goal, *intersection.begin(), i, i);
    }

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

    intersection = backward_expansion([&](const TNode &node) { return graph.predecessors(node); },
                                      forward_data,
                                      backward_data,
                                      goal,
                                      i + 1,
                                      f_user,
                                      f_stop); // odd run

    // If there is a intersection, then we can reconstruct path
    if (intersection.size() != 0) {
      return constructPath(graph, start, goal, *intersection.begin(), i, i + 1);
    }
  }

  return ext::vector<TNode>();
}

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

template<typename FSucc, typename TNode, typename F1, typename F2>
ext::set<TNode> IDDFS::backward_expansion(FSucc successors,
                                          const IDDFS::Data<TNode> &forward_data,
                                          IDDFS::Data<TNode> &backward_data,
                                          const TNode &node,
                                          unsigned long max_depth,
                                          F1 f_user,
                                          F2 f_stop) {
  expansion(successors, backward_data, node, true, max_depth, f_user, f_stop);

  // Check for intersection
  ext::set<TNode> intersection;
  std::set_intersection(forward_data.v.begin(), forward_data.v.end(),
                        backward_data.v.begin(), backward_data.v.end(),
                        std::inserter(intersection, intersection.begin()));

  return intersection;
}

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

template<typename TGraph, typename TNode>
ext::vector<TNode> IDDFS::constructPath(const TGraph &graph,
                                        const TNode &start,
                                        const TNode &goal,
                                        TNode intersection,
                                        unsigned long max_forward_depth,
                                        unsigned long max_backward_depth) {
  Data<TNode> forward_data;
  Data<TNode> backward_data;
  init(forward_data, start);
  init(backward_data, goal);

  // Re-run forward search
  expansion([&](const TNode &node) { return graph.successors(node); },
            forward_data,
            start,
            false,
            max_forward_depth,
            [](const TNode &, const size_t &) -> bool { return false; },
            [&](const TNode &n) -> bool { return n == intersection; }
  );

  // Re-run backward search
  expansion([&](const TNode &node) { return graph.predecessors(node); },
            backward_data,
            goal,
            false,
            max_backward_depth,
            [](const TNode &, const size_t &) -> bool { return false; },
            [&](const TNode &n) -> bool { return n == intersection; }
  );

  return common::ReconstructPath::joinPath(forward_data.path, backward_data.path);
}

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

template<typename TNode>
void IDDFS::init(IDDFS::Data<TNode> &data, const TNode &start) {
  // Make sure that data are wiped
  data.path.clear();
  data.v.clear();

  data.path.push_back(start);
  data.path_set.insert(start);
}

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

} // namespace traverse

} // namespace graph

