#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/astar_search.hpp>

// Un waypoint
struct WayPoint{
    Vector3f pos;
    // et éventuellement d'autres informations (crouch, hide, camp, ...)
};
 
// Une liaison entre deux waypoints
struct WayPointConnection{
    float dist;
    // idem
};

typedef boost::adjacency_list<  // adjacency_list est un template qui dépend de :
    boost::listS,               //  le conteneur utilisé pour les arcs partant d'un sommet. Ici, std::list.
    boost::vecS,                //  le conteneur utilisé pour contenir tous les sommets du graphe. Ici, std::vector.
    boost::undirectedS,         //  le type des arcs. Pourrait être boost::directedS.
    WayPoint,                   //  le type qui décrit un sommet.
    WayPointConnection          //  le type qui décrit un arc.
> WayPointGraph;

typedef WayPointGraph::vertex_descriptor WayPointID;
typedef WayPointGraph::edge_descriptor   WayPointConnectionID;

WayPointGraph graphe;

for (tous nos sommets S){
    WayPointID wpID = boost::add_vertex(graphe); // wp est l'indice d'un nouveau sommet qui a été ajouté dans graphe
    graphe[wpID].pos = la position du sommet S    // graphe[ un WayPointID ] est un WayPoint. On peut donc régler sa position de cette manière.
}

for (tous nos sommets U){ // petit changement de notation, dans boost un arc est souvent décrit entre deux sommets u et v.
    for (tous les voisins V de U){ // C'est à vous de savoir quels sont les voisins de U.
 
        WayPointID u = indice de U; // soit c'est le int du for(), soit il faut y accéder via un autre vector. Cf les notes plus bas.
        WayPointID v = indice de V;
 
        WayPointConnectionID edge;
        bool ok;
        boost::tie(edge, ok) = boost::add_edge(u,v, graphe); // boost::add_edge renvoie une std::pai>WayPointConnectionID,bool>. C'est compliqué à écrire, alors on laisse boost::tie le faire pour nous.
 
        if (ok)  // Si le graphe a bel et bien été ajouté ( pas de doublon, par exemple, sauf si spécifié dans le typedef de WayPointGraph )
        graphe[edge].dist = Distance(graphe[u].pos, graphe[v].pos); // Longueur d'un arc = distance euclidienne entre les deux sommets u et v.
 
    }
}

for (int i=0; i<indices.size()/3; i++){
    ConnectiviteTriangle connectivity;
    fillconnectivityinfo(i, connectivity, indices);
    for (int c=0; c!=3; c++){
        if (connectivity.connexe[c]!=-1){
        TriangleDescriptor u = i;
        TriangleDescriptor v = connectivity.connexe[c];

// Une instance de cette structure sera lancée en exception quand on aura trouvé un chemin
struct found_goal {};
 
// Le visiteur, dont le but est de définir une fonction examine_vertex qui dit si on est arrivé au but.
// Le but est spécifié via le constructeur.
class astar_goal_visitor : public boost::default_astar_visitor{
public:
    astar_goal_visitor(WayPointID goal) : m_goal(goal) {}
 
    void examine_vertex(WayPointID u, const Graph & g){ // Le const est important.
        if(u == m_goal)
            throw found_goal(); // On sort en lancant une exception. C'est moche mais c'est comme ça.
        }
private:
    WayPointID m_goal;
};

TriangleDescriptor goal = ...
TriangleDescriptor start = ...

// Deux vecteurs qui vont servir à sauvegarder le résultat de la recherche
vector<WayPointID> p(boost::num_vertices(graphe)); // Les prédécesseurs
vector<float>      d(boost::num_vertices(graphe)); // Les distances
 
try { // Encore une fois, la découverte d'un chemin est signalée par une exception, donc il faut un try/catch.
    boost::astar_search
    (
        graphe, // notre graphe
        start,  // notre point de départ
        boost::astar_heuristic<NavMeshGraph, float>(), // Une heuristique bidon qui renvoie toujours 0
        boost::predecessor_map(&p[0]).distance_map(&d[0]).visitor(astar_goal_visitor(goal)).weight_map(boost::get(&WayPointConnection::dist, graphe)) // Voir plus bas
    );
 
} catch(found_goal fg) { // On a trouvé un chemin
    // On suit le chemin trouvé dans le sens inverse
    std::list<WayPointID> shortest_path;
    for(WayPointID v = goal;; v = p[v]) {
        shortest_path.push_front(v);
        if(p[v] == v)
            break;
    }
 
    // et on l'affiche.
    std::list<WayPointID>::iterator spi = shortest_path.begin();
    for(++spi; spi != shortest_path.end(); ++spi)
        Log(*spi);
    Log("Total travel time: " , d[goal] );
}

class distance_heuristic : public boost::astar_heuristic <WayPointGraph, float>{
public:
    // De la même manière que tout à l'heure, on passe les données dont il aura besoin dans son foncteur à la construction.
    distance_heuristic(const WayPointGraph & l, WayPointID goal)
    : m_graph(l), m_goal(goal) {}
 
    float operator()(WayPointID u)
    {
        const WayPoint & U = m_graph[u];
        const WayPoint & V = m_graph[m_goal];
        float dx = U.pos.x - V.pos.x;
        float dy = U.pos.y - V.pos.y;
        return sqrt(dx * dx + dy * dy);
    }
private:
    const WayPointGraph & m_graph;
    TriangleDescriptor m_goal;
}