File content as of revision 2:17bd430aeca1:

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

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

private:
    static const float MAX_DIST = 65535; // infinity
    static const uint16_t DEFAULT_VERTEX = 0;
    static const float DEFAULT_ANGLE = 0.0;
    static const int DEFAULT_SIZE = 1;
    //static const int STEPS_PER_INCH = 20; //  per wheel diameter and stepper specs
    static const float PI = 3.14159;

    uint16_t vertex; // current vertex
    float angle; // current angle

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

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

#endif // NAVIGATION_H