File content as of revision 0:fd72f6df078c:

#ifndef NAVIGATION_H
#define NAVIGATION_H
#include <vector>
#include <stack>
#include "stdint.h"
#include "mbed.h"

class Navigation
{
public:
    Navigation(int size);
    void addGraphNode(uint16_t src, uint16_t target, uint16_t dist,
                      uint16_t angle);
    void addGraphNode(uint16_t src);
    int graphSize() { return graph.size(); }
    uint16_t getVertex() { return vertex; }
    uint16_t getAngle() { return angle; }
    void setVertex(uint16_t target) { vertex = target; }
    int numNeighbors(int src) { return graph[src].size(); }
    int loadMap(char* inputFile);
    void getShortestPath(uint16_t destination);
    uint16_t getMinDist(uint16_t target) { return minDistance[target]; }
    std::stack<uint16_t> getRoute(uint16_t src, uint16_t target);
    // utility functions
    void printPrevious(Serial &pc);
    void printRoute(Serial &pc);
    void printGraph(Serial &pc);

private:
    static const uint16_t MAX_DIST = 65535; // infinity
    static const uint16_t DEFAULT_VERTEX = 0;
    static const uint16_t DEFAULT_ANGLE = 0;
    static const int DEFAULT_SIZE = 1;

    uint16_t vertex;
    uint16_t angle;

    struct graphNode
    {
        uint16_t neighbor;
        uint16_t distance;
        uint16_t angle;
        graphNode(uint16_t arg_neighbor = MAX_DIST,
                  uint16_t arg_distance = MAX_DIST,
                  uint16_t arg_angle = MAX_DIST)
           : neighbor(arg_neighbor), distance(arg_distance), angle(arg_angle){}
    };

    std::vector<std::vector<graphNode> > graph;
    std::vector<uint16_t> minDistance;
    std::vector<int> previous;
    std::stack<uint16_t> route;
};

#endif // NAVIGATION_H