Find the n shortest paths from a to b

// compile with: g++ --std=c++11 multipath.cpp 
//
// Problem Statement:
//  find the n shortest paths from a to b

// Answer:
//  The proposer of the question seemed to think this was hard
//  and even stated many more limitations to the question(no cycles etc)..
//  but i took it as dead easy. Since this inst a challenge of efficiency
//  ill stop at the first pass naive recursive answer..

#include <utility>
#include <stdint.h>
#include <vector>
#include <map>
#include <memory>
#include <iostream>

struct Node
{
    uint32_t id_;
    std::vector<std::pair<Node*,uint32_t> > kids_;

    Node(uint32_t id) :
        id_(id)
    {}
};

struct Graph
{
    // meh should be better..
    std::map<uint32_t, std::shared_ptr<Node> > nodes_;

    Node* get(uint32_t id)
    {
        Node* n = nodes_[id].get();

        if (n == NULL)
        {
            nodes_[id] = std::make_shared<Node>(id);
            n = nodes_[id].get();
        }

        return n;
    }

    void link(uint32_t fromId,
              uint32_t toId,
              uint32_t weight)
    {
        Node* from = nodes_[fromId].get();
        if (from == NULL)
        {
            nodes_[fromId] = std::make_shared<Node>(fromId);
            from = nodes_[fromId].get();
        }

        Node* to = nodes_[toId].get();
        if (to == NULL)
        {
            nodes_[toId] = std::make_shared<Node>(toId);
            to = nodes_[toId].get();
        }

        from->kids_.emplace_back(std::make_pair(to,weight));
    }
};

struct PathNode
{
    std::shared_ptr<PathNode> parent_;
    Node*      node_;
    uint32_t   cost_;

    PathNode(std::shared_ptr<PathNode> parent,
             Node*      node,
             uint32_t   cost) :
        parent_(parent),
        node_(node),
        cost_(cost)
    {}
};

std::ostream& operator<<(std::ostream& os,
                         const PathNode& path)
{
    // inverted.. but it meets my needs
    if (path.parent_.get() != NULL)
    {
        const PathNode& p = *(path.parent_);
        os << p;
        os << "-";
    }

    if (path.node_ != NULL)
        os << path.node_->id_;

    return os;
}


// ************ NAIVE APPROACH ***********
// WARNING do be careful here.. there are some large weaknesses
// in the Naive version:
//  - negative weights will break the shortest first ordering
//  - cycles that trap the search in disconnected areas will never terminate
//
// A stronger algo would fix this by checking reachability from the node and
// pruning the impossible branches from the tree (in fact that would be my next
// step in optimization as it also saves cycles.. but im here for a different
// target so not going there.. a more simple fix would be to add a cycle
// limit and terminate on that..)

std::vector<std::shared_ptr<PathNode> > findPaths(Node*    from,
                                                  Node*    to,
                                                  uint32_t limit)
{
    std::vector<std::shared_ptr<PathNode> > results;
    auto comp = [] (const std::shared_ptr<PathNode>& a,
                    const std::shared_ptr<PathNode>& b) -> bool
        { return a->cost_ > b->cost_; };

    // I was using a std::priority_queue.. problem is i
    // cant print it to demonstrate the stack action to
    // the question proposer
    std::vector<std::shared_ptr<PathNode> > stack;

    // ok start the algo by adding the *from* node into a path.. and stacking it
    stack.push_back(std::make_shared<PathNode>(nullptr,
                                               from,
                                               0));

    while (results.size() < limit and not stack.empty())
    {
        // show the stack...
        // std::cout << "  stack: \n";
        // for (const std::shared_ptr<PathNode>& path : stack)
        //     std::cout << " -- c:" << path->cost_ << " p:" << *path << "\n";

        std::pop_heap(stack.begin(), stack.end(), comp);
        std::shared_ptr<PathNode> current = stack.back();
        stack.pop_back();

        // check if node was at the terminal
        if (current->node_ == to)
        {
            // register it as the next result
            results.push_back(current);

            // and end if we have our "limit" of solutions
            if (results.size() > limit)
                return results;
        }

        // and then expand search tree using the current node
        for (const std::pair<Node*,uint32_t>& edge : current->node_->kids_)
        {
            std::shared_ptr<PathNode> step =
                std::make_shared<PathNode>(current,
                                           edge.first,
                                           current->cost_ + edge.second);

            // now here is a tricky part.. the next shortest route may
            // include the current one even if it is a *terminal* (ie self loop)
            // hence we *always* push even if its the *end*!
            stack.push_back(step);
            std::push_heap(stack.begin(), stack.end(), comp);
        }
    }

    // ok we fell of the end of the stack that means there are not "limit" solutions
    return results;
}

void printPaths(std::ostream& os,
                const std::vector<std::shared_ptr<PathNode> >& paths)
{
    os << "Results...\n";
    for (const std::shared_ptr<PathNode>& path : paths)
    {
        os << "d:" << path->cost_ << " p:" << *path << "\n";
    }
}

void test()
{
    {
        // warning naive algo death - dont do this with a cycle it will never terminate
        Graph disconnected;

        Node* to   = disconnected.get(0);
        Node* from = disconnected.get(1);

        printPaths(std::cout, findPaths(from,to,3));
    }

    {
        // self
        Graph self;
        self.link(0,0,1);

        Node* zero = self.get(0);

        printPaths(std::cout, findPaths(zero,zero,3));
    }

    {
        // cycle
        Graph cycle;
        cycle.link(0,1,1);
        cycle.link(1,0,1);

        Node* from = cycle.get(0);
        Node* to   = cycle.get(1);

        printPaths(std::cout, findPaths(from,to,3));
    }

    {
        // a line of nodes ( asking for 3 anwers.. impossible)
        Graph oneLine;
        oneLine.link(0,1,1);
        oneLine.link(1,2,1);
        oneLine.link(2,3,1);

        Node* from = oneLine.get(0);
        Node* to   = oneLine.get(3);

        printPaths(std::cout, findPaths(from,to,3));
    }

    {
        // 2 lines of nodes ( asking for 3 anwers.. impossible)
        Graph twoLine;
        twoLine.link(0,1,1);
        twoLine.link(1,2,1);
        twoLine.link(2,3,1);
        twoLine.link(0,4,2);
        twoLine.link(4,3,2);

        Node* from = twoLine.get(0);
        Node* to   = twoLine.get(3);

        printPaths(std::cout, findPaths(from,to,3));
    }

    {
        // the questioners challenge graph
        Graph triangle;
        triangle.link(1,2,1);
        triangle.link(1,3,2);

        triangle.link(2,1,1);
        triangle.link(2,3,3);
        triangle.link(2,4,2);
        triangle.link(2,5,1);

        triangle.link(3,1,2);
        triangle.link(3,2,3);
        triangle.link(3,5,2);
        triangle.link(3,6,1);

        triangle.link(4,2,2);
        triangle.link(4,5,3);

        triangle.link(5,2,1);
        triangle.link(5,3,2);
        triangle.link(5,4,3);
        triangle.link(5,6,3);

        triangle.link(6,5,3);
        triangle.link(6,3,1);

        Node* from = triangle.get(1);
        Node* to   = triangle.get(6);

        printPaths(std::cout, findPaths(from,to,10));
    }
}

int main()
{
    test();
}

And output will look like this:

Results...
Results...
d:0 p:0
d:1 p:0-0
d:2 p:0-0-0
Results...
d:1 p:0-1
d:3 p:0-1-0-1
d:5 p:0-1-0-1-0-1
Results...
d:3 p:0-1-2-3
Results...
d:3 p:0-1-2-3
d:4 p:0-4-3
Results...
d:3 p:1-3-6
d:5 p:1-2-3-6
d:5 p:1-2-1-3-6
d:5 p:1-2-5-6
d:5 p:1-3-6-3-6
d:5 p:1-2-5-3-6
d:7 p:1-2-1-2-5-6
d:7 p:1-2-1-2-1-3-6
d:7 p:1-2-3-6-3-6
d:7 p:1-2-5-2-1-3-6