// ReconstructPath.hpp
//
//     Created on: 19. 12. 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

#pragma once

#include <alib/map>
#include <alib/vector>
#include <algorithm>

namespace graph::common {

class ReconstructPath {
// ---------------------------------------------------------------------------------------------------------------------
 public:
  template<typename TNode>
  static ext::vector<TNode> reconstructPath(const ext::map<TNode, TNode> &p,
                                            const TNode &start,
                                            const TNode &goal);

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

  template<typename TNode>
  static ext::vector<TNode> reconstructPath(const ext::map<TNode, TNode> &p_forward,
                                            const ext::map<TNode, TNode> &p_backward,
                                            const TNode &start,
                                            const TNode &goal,
                                            const TNode &intersection_node);

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

  template<typename TNode, typename TWeight>
  static
  ext::pair<ext::vector<TNode>, TWeight>
  reconstructWeightedPath(const ext::map<TNode, TNode> &p,
                          const ext::map<TNode, TWeight> &g,
                          const TNode &start,
                          const TNode &goal);

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

  template<typename TNode, typename TWeight>
  static
  ext::pair<ext::vector<TNode>, TWeight>
  reconstructWeightedPath(const ext::map<TNode, TNode> &p_forward,
                          const ext::map<TNode, TNode> &p_backward,
                          const ext::map<TNode, TWeight> &g_forward,
                          const ext::map<TNode, TWeight> &g_backward,
                          const TNode &start,
                          const TNode &goal,
                          const TNode &intersection_node);

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

  template<typename TNode>
  static
  ext::vector<TNode>
  joinPath(ext::vector<TNode> &forward_path, const ext::vector<TNode> &backward_path);

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

template<typename TNode>
ext::vector<TNode> ReconstructPath::reconstructPath(const ext::map<TNode, TNode> &p,
                                                    const TNode &start,
                                                    const TNode &goal) {
  ext::vector<TNode> path;

  // Path not found -> return empty vector
  if (p.find(goal) == p.end()) {
    return path;
  }

  path.push_back(goal);
  TNode current_vertex = goal;

  while (current_vertex != start) {
    current_vertex = p.at(current_vertex);
    path.push_back(current_vertex);
  }

  // Path need to be reverse.
  std::reverse(path.begin(), path.end());

  return path;
}

template<typename TNode>
ext::vector<TNode> ReconstructPath::reconstructPath(const ext::map<TNode, TNode> &p_forward,
                                                    const ext::map<TNode, TNode> &p_backward,
                                                    const TNode &start,
                                                    const TNode &goal,
                                                    const TNode &intersection_node) {
  ext::vector<TNode> path;

  // Path not found -> return empty vector
  if (p_forward.find(intersection_node) == p_forward.end()
      || p_backward.find(intersection_node) == p_backward.end()) {
    return path;
  }

  // First part of the path. From start to intersectionVertex (include).
  path.push_back(intersection_node);
  TNode current_vertex = intersection_node;

  while (current_vertex != start) {
    current_vertex = p_forward.at(current_vertex);
    path.push_back(current_vertex);
  }
  // This part of the path need to be reverse.
  std::reverse(path.begin(), path.end());

  // Second part of the path. From intersectionVertex (exclude) to goal.
  current_vertex = intersection_node;
  while (current_vertex != goal) {
    current_vertex = p_backward.at(current_vertex);
    path.push_back(current_vertex);
  }
  // No reverse at the end.

  return path;
}

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

template<typename TNode, typename TWeight>
ext::pair<ext::vector<TNode>, TWeight>
ReconstructPath::reconstructWeightedPath(const ext::map<TNode, TNode> &p,
                                         const ext::map<TNode, TWeight> &g,
                                         const TNode &start,
                                         const TNode &goal) {
  if (g.find(goal) == g.end()) {
    return ext::make_pair(ext::vector<TNode>(), std::numeric_limits<TWeight>::max());
  }

  return ext::make_pair(reconstructPath(p, start, goal), g.at(goal));
}

template<typename TNode, typename TWeight>
ext::pair<ext::vector<TNode>, TWeight>
ReconstructPath::reconstructWeightedPath(const ext::map<TNode, TNode> &p_forward,
                                         const ext::map<TNode, TNode> &p_backward,
                                         const ext::map<TNode, TWeight> &g_forward,
                                         const ext::map<TNode, TWeight> &g_backward,
                                         const TNode &start,
                                         const TNode &goal,
                                         const TNode &intersection_node) {
  if (g_forward.find(intersection_node) == g_forward.end() || g_backward.find(intersection_node) == g_backward.end()) {
    return ext::make_pair(ext::vector<TNode>(), std::numeric_limits<TWeight>::max());
  }

  return ext::make_pair(reconstructPath(p_forward, p_backward, start, goal, intersection_node),
                        g_forward.at(intersection_node) + g_backward.at(intersection_node));
}

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

template<typename TNode>
ext::vector<TNode> ReconstructPath::joinPath(ext::vector<TNode> &forward_path,
                                             const ext::vector<TNode> &backward_path) {
  // ++ because we want skip the insertion node -> already in forward_path
  forward_path.insert(forward_path.end(), ++backward_path.rbegin(), backward_path.rend());
  return forward_path;
}

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

} // namespace graph::common

