File content as of revision 1:a53d97b74fab:

#include "StepperMotor.h"
#include "navigation.h"
#include "stdint.h"
#include <fstream>
#include <stack>
#include <set>

// FUNCTION:
//      Navigation(int size = DEFAULT_SIZE)
// IN-PARAMETERS:
//      Number of initial vertices (DEFAULT = 1)
// OUT-PARAMETERS:
//      None
// DESCRIPTION:
//      Default and 1-parameter constructor.
Navigation::Navigation(int size = DEFAULT_SIZE) : vertex(DEFAULT_VERTEX),
                                                        angle(DEFAULT_ANGLE)
{
    graph.resize(size);
}

// FUNCTION:
//      addGraphNode(uint16_t src, uint16_t target, uint16_t dist,
//                   uint16_t angle)
// IN-PARAMETERS:
//      Source node (uint16_t), target node (uint16_t), edge distance
//      (uint16_t), and angle (uint16_t)
// OUT-PARAMETERS:
//      None
// DESCRIPTION:
//      Adds a graph node to the graph.
void Navigation::addGraphNode(uint16_t src, uint16_t target, uint16_t dist,
                                 uint16_t angle)
{
    graph[src].push_back(graphNode(target, dist, angle));
}

// FUNCTION:
//      addGraphNode(uint16_t src, uint16_t target, uint16_t dist,
//                   uint16_t angle)
// IN-PARAMETERS:
//      Source node (uint16_t)
// OUT-PARAMETERS:
//      None
// DESCRIPTION:
//      Adds a graph node to the graph, with target, dist, and angle
//      initialized to infinity
void Navigation::addGraphNode(uint16_t src)
{
    graph[src].push_back(graphNode());
}

// FUNCTION:
//      int loadMap(char* infile);
// IN-PARAMETERS:
//      Input file name (*char[])
// OUT-PARAMETERS:
//      Returns 1 if file doesn't open
//      Returns 0 if file opens succesfully
// DESCRIPTION:
//      Loads contents of a file as a weighted graph. First entry in the file
//      should be the number of vertices. All subsequent entries indicate edges
//      by listing a source vertex, a target vertex, the distance between them
//      and the angle from source to target (0 - 359 degrees) . Only edges
//      are listed. Assumes that inverse angles/edges are included in file.
int Navigation::loadMap(char* inputFile)
{
    std::ifstream inFile;
    inFile.open(inputFile);
    if (!inFile)
        return 1; // returns 1 if file fails to open

    int vertices;
    inFile >> vertices; // reads first entry as number of vertices
    graph.resize(vertices); // resizes vector as necessary

    uint16_t src;
    uint16_t target;
    uint16_t dist;
    uint16_t angle;

    while (!inFile.eof() )
    {
        inFile >> src;
        inFile >> target;
        inFile >> dist;
        inFile >> angle;

        addGraphNode(src, target, dist, angle);
    } // loads all data from file into graph

    return 0;
}

// FUNCTION:
//      void getShortestPath(uint16_t src, uint16_t destination)
// IN-PARAMETERS:
//      Target vertex (uint16_t)
// OUT-PARAMETERS:
//      None
// DESCRIPTION:
//      Implementation of Dijkstra's algorithm. Uses current vertex as source
//      and calculates shortest path information for all other vertices.
//      Subroutine ends when shortest path for target vertex has been found
//      and then loads the path into route.
void Navigation::getShortestPath(uint16_t destination)
{
    uint16_t src = vertex;
    uint16_t v; // stores temporary vertex
    int n = graph.size();
    minDistance.clear();
    minDistance.resize(n, MAX_DIST);
    minDistance[src] = 0;
    previous.clear();
    previous.resize(n, -1);
    std::set<std::pair<uint16_t, uint16_t> > vertex_queue;
    vertex_queue.insert(std::make_pair(minDistance[src], src));

    while (!vertex_queue.empty())
    {
        uint16_t dist = vertex_queue.begin()->first;
        uint16_t u = vertex_queue.begin()->second;
        vertex_queue.erase(vertex_queue.begin());

        // Visit each edge exiting u
        const std::vector<graphNode> &neighbors = graph[u];
        for (std::vector<graphNode>::const_iterator neighbor_iter = neighbors.begin();
             neighbor_iter != neighbors.end();
             neighbor_iter++)
        {
            v = neighbor_iter->neighbor;
            uint16_t weight = neighbor_iter->distance;
            uint16_t distance_through_u = dist + weight;

            if (distance_through_u < minDistance[v])
            {
                vertex_queue.erase(std::make_pair(minDistance[v], v));

                minDistance[v] = distance_through_u;
                previous[v] = u;
                vertex_queue.insert(std::make_pair(minDistance[v], v));
            }
            if (v == destination)
                break;
        }
        if (v == destination)
            break;
    }
    while (v != src)
    {
        route.push(v);
        v = previous[v];
    }
}

// FUNCTION:
//      void executeRoute()
// IN-PARAMETERS:
//      Address of left and right steppers (StepperMotor &)
// OUT-PARAMETERS:
//      None
// DESCRIPTION:
//      Sends command to steppers to execute route.
void Navigation::executeRoute(StepperMotor &leftMotor, StepperMotor &rightMotor)
{
    uint16_t target;
    uint16_t targetAngle;
    uint16_t targetDist;
    
    while (!route.empty() )
    {
        target = route.top();
        route.pop();
        
        for (int i = 0; graph[vertex][i].neighbor == target; i++)
    }
}

// utility function
void Navigation::printPrevious(Serial &pc)
{
    for(int i = 0; i < previous.size(); i++)
    {
        pc.printf("Node %i is prev to node %i\n", previous[i], i);
    }
}

// utility function
void Navigation::printRoute(Serial &pc)
{
    while (!route.empty() )
    {
        pc.printf("Go to node %i\n", route.top() );
        route.pop();
    }
}

// utility function
void Navigation::printGraph(Serial &pc)
{
    for (int i = 0; i < graph.size(); i++)
        for (int j = 0; j < graph[i].size(); j++)
        {
            pc.printf("Node %i to %i = dist - %i, angle - %i\n", i, j, graph[i][j].distance, graph[i][j].angle);
        }
}