// IDAStar.hpp
//
//     Created on: 08. 03. 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

#ifndef ALIB2_IDASTAR_HPP
#define ALIB2_IDASTAR_HPP

#include <functional>
#include <alib/vector>
#include <alib/set>
#include <alib/pair>
#include <limits>
#include <algorithm>
#include <stdexcept>

#include <common/ReconstructPath.hpp>
#include <common/SupportFunction.hpp>

using namespace std;

namespace graph {

namespace shortest_path {

class IDAStar {
// ---------------------------------------------------------------------------------------------------------------------
 public:

  /// Find the shortest path using IDAStar algorithm from the \p start node to the \p goal node in the \p graph.
  ///
  /// The heuristic function must be admissible and monotone.
  ///
  /// The heuristic function must be admissible and monotone.
  ///
  /// \param graph to explore.
  /// \param start initial node.
  /// \param goal final node.
  /// \param f_heuristic heuristic function which accept node and return edge_type::weight_type.
  /// \param f_user function which is called for every opened node with value of currently shortest path.
  ///
  /// \returns pair where first := shortest path := distance of path, if there is no such path vector is empty and distance std::numeric_limits<edge_type:weight_type>::max().
  ///
  /// \note TEdge of \p graph must follow graph::edge::WeightedEdge interface.
  /// \sa graph::edge_type::WeightedEdge.
  ///
  /// \throws std::out_of_range if \p graph contains an edge with a negative weight.
  ///
  template<
      typename TGraph,
      typename TNode,
      typename F1 = std::function<typename TGraph::edge_type::weight_type(const TNode &)>,
      typename F2 = std::function<void(const TNode &, const typename TGraph::edge_type::weight_type &)>>
  static
  ext::pair<ext::vector<TNode>, typename TGraph::edge_type::weight_type>
  findPath(const TGraph &graph,
           const TNode &start,
           const TNode &goal,
           F1 f_heuristic,
           F2 f_user = [](const TNode &,
                          const typename TGraph::edge_type::weight_type &) {});

  template<
      typename TGraph,
      typename TNode,
      typename F1 = std::function<typename TGraph::edge_type::weight_type(const TNode &, const TNode &)>
  >
  static
  ext::pair<ext::vector<TNode>, typename TGraph::edge_type::weight_type>
  findPathRegistration(const TGraph &graph,
                       const TNode &start,
                       const TNode &goal,
                       F1 f_heuristic) {
    return findPath(graph, start, goal, [&](const TNode &n) { return f_heuristic(goal, n); });

  }

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

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

 private:

  template<typename TNode, typename TWeight>
  struct Data {
    ext::vector<TNode> path; // current path
    ext::set<TNode> path_set; // current path set for quick search
    TWeight path_size; // size of found path
  };

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

  template<typename TGraph, typename TNode, typename F1, typename F2>
  static
  ext::pair<bool, typename TGraph::edge_type::weight_type>
  search(const TGraph &graph,
         IDAStar::Data<TNode, typename TGraph::edge_type::weight_type> &data,
         const TNode &goal,
         typename TGraph::edge_type::weight_type gscore,
         typename TGraph::edge_type::weight_type bound,
         F1 f_heuristic,
         F2 f_user);

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

};

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

template<typename TGraph, typename TNode, typename F1, typename F2>
ext::pair<ext::vector<TNode>, typename TGraph::edge_type::weight_type>
IDAStar::findPath(const TGraph &graph,
                  const TNode &start,
                  const TNode &goal,
                  F1 f_heuristic,
                  F2 f_user) {
  using weight_type = typename TGraph::edge_type::weight_type;

  Data<TNode, weight_type> data;
  data.path.push_back(start);
  data.path_set.insert(start);

  weight_type bound = f_heuristic(start);
  while (1) {
    bool found;
    weight_type t;
    std::tie(found, t) = search(graph, data, goal, 0, bound, f_heuristic, f_user);

    if (found) {
      return ext::make_pair(data.path, data.path_size); // Return found path
    } else if (t == std::numeric_limits<weight_type>::max()) {
      return ext::make_pair(ext::vector<TNode>(), std::numeric_limits<weight_type>::max()); // No path exists
    }

    bound = t;
  }
}

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

template<typename TGraph, typename TNode, typename F1, typename F2>
ext::pair<bool, typename TGraph::edge_type::weight_type>
IDAStar::search(const TGraph &graph,
                IDAStar::Data<TNode, typename TGraph::edge_type::weight_type> &data,
                const TNode &goal,
                typename TGraph::edge_type::weight_type gscore,
                typename TGraph::edge_type::weight_type bound,
                F1 f_heuristic,
                F2 f_user) {
  using weight_type = typename TGraph::edge_type::weight_type;

  TNode n = data.path.back();
  weight_type f = gscore + f_heuristic(n);

  if (f > bound) {
    return ext::make_pair(false, f);
  }

  // Run user function
  f_user(n, gscore);

  // Check for goal
  if (n == goal) {
    data.path_size = gscore;
    return ext::make_pair(true, f);
  }

  weight_type min = std::numeric_limits<weight_type>::max();
  for (const auto &s_edge: graph.successorEdges(n)) {
    const TNode &s = common::SupportFunction::other(s_edge, n); // successor

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

    // Check for negative edge
    if (s_edge.weight() < 0) {
      throw std::out_of_range("IDAStar: Detect negative weight on edge in graph.");
    }

    bool found;
    weight_type t;

    data.path.push_back(s); // Insert node to the path
    data.path_set.insert(s);
    std::tie(found, t) =
        search(graph, data, goal, gscore + s_edge.weight(), bound, f_heuristic, f_user); // Run recursion

    if (found) {
      return ext::make_pair(true, t);
    } else if (t < min) {
      min = t; // Update min
    }

    data.path.pop_back(); // Pop node from the path
    data.path_set.erase(data.path_set.find(s));
  }

  return ext::make_pair(false, min);
}

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

} // namespace shortest_path

} // namespace graph

#endif //ALIB2_IDASTAR_HPP
