/**
 * @file Astar.h
 * @brief A*-search algorithm implementation
 * @author Dmitri Kuvshinov
 */
#pragma once

#include <functional>
#include <set>
#include <vector>


/*
Компаратор двух индексов, сравнивающий их в первую очередь по
соответствующим значениям некоторого массива с помощью компаратора Cmp1,
если же эти значения эквивалентны для Cmp1, то 
сравниваются непосредственно сами индексы с помощью компаратора Cmp2.
Второе сравнение нужно для того, чтобы набор индексов мог включать
любой индекс не более одного раза, но даже в том случае, когда
значения по индексам совпадают (эти значения нужны для упорядочивания).
*/

template< class T, class Arr, class Cmp1, class Cmp2 = std::less<T> >
class ByArrayComparator
  : public std::binary_function<T, T, bool>
{
  const Arr *arr;
  Cmp1 cmp1;
  Cmp2 cmp2;

public:
  ByArrayComparator() {}

  explicit ByArrayComparator(const Arr &a, Cmp1 cmp1 = Cmp1(), Cmp2 cmp2 = Cmp2())
    : arr(&a), cmp1(cmp1), cmp2(cmp2) {}

  bool operator()(const T a, const T b) const
  {
    return cmp1( (*arr)[a], (*arr)[b]) || 
              !cmp1((*arr)[b], (*arr)[a]) && cmp2(a, b);
  }
};


/*
Алгоритм A* осуществляет поиск кратчайшего пути между двумя заданными вершинами
с использованием эвристики (быстро оценивающей расстояние между двумя вершинами графа)
для выбора приоритетного направления движения на каждом шаге.

Для перебора соседей используется вектор наборов соседей AdjVec,
для получения "длины" ребра -- бинарный функтор WeightMatrix.
*/

template
  <
    class Previous
  , class WeightMatrix
  , class AdjVec
  , class Heuristic
  , class DemoPolicy
  >
bool astar
  (
    Previous &Pr          // набор предшественников, позволяющий восстановить найденный путь
  , const WeightMatrix &W // длины рёбер
  , const AdjVec &A       // представление графа в виде вектора наборов соседей
  , const unsigned start  // индекс начальной вершины
  , const unsigned goal   // индекс конечной вершины
  , const unsigned N      // количество вершин в графе
  , Heuristic f           // эвристика -- оценка расстояния между парой вершин
  , DemoPolicy demo       // генератор событий для визуализации алгоритма
  )
{
  // удобные синонимы типов
  typedef typename Heuristic::result_type Distance;
  typedef std::vector<Distance> Array;
  typedef ByArrayComparator< unsigned, Array, std::less<Distance> > Comp;
  typedef typename AdjVec::value_type Adjacency;    // набор соседей
  typedef typename Adjacency::const_iterator AdjacencyIt; // итератор набора соседей

  // инициализация массивов
  Array gscore(N), hscore(N), fscore(N);
  gscore[start] = Distance(0);
  fscore[start] = hscore[start] = f(start, goal);
  Pr[start] = start;

  // наборы "закрытых" и "открытых" вершин
  std::vector<bool> closedset(N);
  std::set<unsigned, Comp> openset( (Comp(fscore)) ); // упорядочивать в силу fscore
   
  // начать с начальной вершины, продолжать, пока есть "открытые"
  openset.insert(start);
    // -- demo --
    demo.openVertex(start, fscore[start]);
    
  while (!openset.empty())
  {
    // получить индекс следующей вершины, закончить, если это -- конечная вершина
    const unsigned x = *openset.begin();
    if (x == goal)
      return true;

    // переместить вершину из набора "открытых" в набор "закрытых"
    openset.erase(openset.begin());
    closedset[x] = true;
      // -- demo --
      demo.closeVertex(x);
    
    // исследовать соседей вершины
    for (AdjacencyIt py = A[x].begin(), pyend = A[x].end(); py != pyend; ++py)
    {
      const unsigned y = *py; // индекс очередного соседа
      if (!closedset[y]) // если его нет в наборе "закрытых", то
      {
        // оценка длины пути, проходящего по ребру (x, y)
        const Distance possible_gscore = gscore[x] + W(x, y);

        const auto p = openset.find(y);
        const bool yNotFound = p == openset.end() // есть ли эта вершина среди "открытых"?
                 , possibleIsLower = possible_gscore < gscore[y]; // может ли вход из x сократить
                                                                  // длину пути, проходящего через y?

        // если то или иное -- истина, то
        if (yNotFound || possibleIsLower)
        {
          // удалить вершину y из "открытых", если y там была раньше
          if (!yNotFound) // (чтобы корректно переупорядочить вершины из-за изменения fscore)
            openset.erase(p);
          
          // путь, включающий y, пойдёт туда через x, пересчитать массивы
          Pr[y] = x;
          gscore[y] = possible_gscore;
          hscore[y] = f(y, goal);
          fscore[y] = gscore[y] + hscore[y];
          // поместить y на новое место в соответствии с fscore
          openset.insert(y);
            // -- demo --
            demo.openVertex(y, fscore[y]);
        }
      }
    }
  }

  // так и не удалось достичь конечной вершины
  return false;
}


/*
Вариант с политикой "по умолчанию".
*/

template<class Previous, class WeightMatrix, class AdjVec, class Heuristic>
bool astar
  (
    Previous &Pr
  , const WeightMatrix &W
  , const AdjVec &A
  , const unsigned start
  , const unsigned goal
  , const unsigned N
  , Heuristic f 
  )
{
  /*
  Политика генерации событий демонстрации алгоритма,
  используемая по умолчанию. Ничего не делает.
  */
  struct AstarDefaultDemoPolicy
  {
    void openVertex(unsigned v, float score) {}
    void closeVertex(unsigned v) {}
  } doNothing;
  return astar(Pr, W, A, start, goal, N, f, doNothing);
}