/**
 * @file a_star_algorithm_imp.hpp
 *
 * CopyRight F. J. Madrid-Cuevas <fjmadrid@uco.es>
 *
 * Sólo se permite el uso de este código en la docencia de las asignaturas sobre
 * Estructuras de Datos de la Universidad de Córdoba.
 *
 * Está prohibido su uso para cualquier otro objetivo.
 */
#pragma once

#include <tuple>
#include <limits>
#include <functional>
#include <queue>

#include <a_star_algorithm.hpp>

template <class T>
size_t
a_star_algorithm(WGraph<T> &g,
                 size_t start,
                 size_t end,
                 WGraphHeuristic<T> &H,
                 std::vector<size_t> &predecessors,
                 std::vector<float> &distances)
{
    assert(start < g.order());
    assert(end < g.order());

    using Tuple = std::tuple<float, float, size_t, size_t>;

    size_t iterations = 0;
    g.reset(false);

    size_t n = g.order();
    predecessors.resize(n);
    distances.resize(n);
    for (size_t i = 0; i < n; ++i)
    {
        predecessors[i] = i;
        distances[i] = std::numeric_limits<float>::infinity();
    }
    distances[start] = 0.0f;

    std::priority_queue<Tuple, std::vector<Tuple>, std::greater<Tuple>> pq;
    float h0 = H(g.item(start), g.item(end));
    pq.push({h0, 0.0f, start, start});

    while (!pq.empty())
    {
        float f     = std::get<0>(pq.top());
        float d     = std::get<1>(pq.top());
        size_t u    = std::get<2>(pq.top());
        size_t pred = std::get<3>(pq.top());
        pq.pop();
        (void)f;

        ++iterations;

        if (g.is_visited(u))
            continue;
        g.set_visited(u, true);
        predecessors[u] = pred;
        distances[u] = d;

        if (u == end)
            break;

        auto u_it = g.vertex(u);
        for (auto e_it = g.edges_begin(u_it); e_it != g.edges_end(u_it); ++e_it)
        {
            size_t v = e_it->second()->label();
            if (!g.is_visited(v))
            {
                float new_g = d + e_it->item();
                float new_h = H(g.item(v), g.item(end));
                pq.push({new_g + new_h, new_g, v, u});
            }
        }
    }

    return iterations;
}

inline std::list<size_t>
a_star_path(size_t src, size_t dst,
            std::vector<size_t> const &predecessors)
{
    assert(src < predecessors.size());
    assert(dst < predecessors.size());
    assert(predecessors[src] == src);

    std::list<size_t> path;

    if (predecessors[dst] == dst && dst != src)
        return path;

    size_t current = dst;
    while (current != src)
    {
        path.push_front(current);
        current = predecessors[current];
    }
    path.push_front(src);

    return path;
}