find_paths.cpp

#include <algorithm>
#include <cmath>
#include <iostream>
#include <map>
#include <memory>
#include <set>
#include <stdexcept>
#include <vector>

#define LARGE_FLOAT 999999.0

struct Step;
using StepPtr = std::shared_ptr<Step>;

struct Neighbor {
  int id;
  double weight;
  Neighbor(int id, double weight) : id(id), weight(weight) {}
};

struct Step {
  int id;
  StepPtr parent;
  // Here, cost is interpreted as the maximum weight encountered so far along
  // the path.
  double cost;
  int depth;  // number of edges in the path

  // Constructor
  Step(int id, StepPtr parent, double cost, int depth)
      : id(id), parent(parent), cost(cost), depth(depth) {}

  // Recursively check if the given node is already in the path.
  bool seen(int node) const {
    if (id == node)
      return true;
    if (!parent)
      return false;
    return parent->seen(node);
  }

  // Recursively generates the path from the start to this node.
  std::vector<int> generatePath() const {
    std::vector<int> path;
    if (parent) {
      path = parent->generatePath();
    }
    path.push_back(id);
    return path;
  }

  // Overload the less-than operator to allow ordering in std::set.
  bool operator<(const Step &other) const {
    // Compare path cost (i.e. maximum weight on the path)
    if (std::abs(cost - other.cost) > 1e-9)
      return cost > other.cost;
    if (depth != other.depth)
      return depth < other.depth;
    if (id != other.id)
      return id < other.id;
    // Finally, compare paths lexicographically.
    std::vector<int> path1 = generatePath();
    std::vector<int> path2 = other.generatePath();
    return path1 < path2;
  }
};

class AstarSearch {
public:
  // Type alias for the adjacency list: node id -> vector of Neighbor.
  using Adjacency = std::map<int, std::vector<Neighbor>>;

  AstarSearch(const Adjacency &adjacency, int source, int destination)
      : adjacency(adjacency), destination(destination) {
    // The initial cost is 0 since no edge has been taken.
    pending.insert(std::make_shared<Step>(source, nullptr, LARGE_FLOAT, 0));
  }

  // Returns the next shortest (i.e. lowest maximum edge weight) path as a
  // vector of ints. Returns an empty vector if no path exists.
  std::vector<int> nextShortestPath() {
    while (!pending.empty()) {
      StepPtr currentStepPtr = *pending.begin();
      pending.erase(pending.begin());
      if (currentStepPtr->id == destination)
        return currentStepPtr->generatePath();

      auto it = adjacency.find(currentStepPtr->id);
      if (it != adjacency.end()) {
        for (const auto &neighbor : it->second) {
          // Avoid cycles.
          if (!currentStepPtr->seen(neighbor.id)) {
            // In this formulation the new path cost is the maximum of the
            // parent's cost and the weight of the current edge.
            // std::cout << currentStepPtr->id << " -> " << neighbor.id << "\n";
            // std::cout << "Current cost: " << currentStepPtr->cost << "\n";
            double newCost = std::min(currentStepPtr->cost, neighbor.weight);
            // std::cout << "Cost before heuristic: " << newCost << "\n";
            // Optionally incorporate a heuristic if one wants to extend this to
            // a full A* search.
            double predicted = predictCost(neighbor.id, destination);
            // Combine the cost using max, since the overall cost is the maximum
            // weight encountered.
            double totalCost = std::min(newCost, predicted);
            // std::cout << "Cost after heuristic: " << totalCost << "\n";
            StepPtr nextStep =
                std::make_shared<Step>(neighbor.id, currentStepPtr, totalCost, currentStepPtr->depth + 1);
            pending.insert(nextStep);
          }
        }
      }
    }
    return std::vector<int>{};
  }

protected:
  // The heuristic function; returns a large number by default.
  // To use a heuristic that better estimates the maximum weight on a remaining
  // path, override this function in a derived class.
  virtual double predictCost(int /*source*/, int /*destination*/) const {
    return LARGE_FLOAT;
  }

private:
  const Adjacency adjacency;
  const int destination;

  // Custom comparator for shared pointers to Steps.
  struct StepPtrCompare {
    bool operator()(const StepPtr &a, const StepPtr &b) const {
      return *a < *b;
    }
  };

  // A set that orders steps by total cost (i.e. the maximum weight encountered
  // so far), then by id and path.
  std::set<StepPtr, StepPtrCompare> pending;
};

//
// Helper routines to create an adjacency list for testing.
//

// Adds a list of neighbors for a given source node.
// The dests vector must contain an even number of numbers: each pair
// {destination, weight}.
void addAdjacency(AstarSearch::Adjacency &adjacency, int source,
                  const std::vector<double> &dests) {
  if (dests.size() % 2 != 0)
    throw std::invalid_argument("dests must have an even number of arguments "
                                "(pairs of destination and weight)");
  std::vector<Neighbor> neighbors;
  for (size_t i = 0; i < dests.size(); i += 2) {
    // Convert the destination id from double to int.
    int destId = static_cast<int>(dests[i]);
    double weight = dests[i + 1];
    neighbors.emplace_back(destId, weight);
  }
  adjacency[source] = neighbors;
}

AstarSearch::Adjacency createAdjacency() {
  AstarSearch::Adjacency adjacency;
  // Example graph: the weights here are given as doubles.
  // For instance, for node 1: edges (1->2) with weight 0.5 and (1->5) with
  // weight 0.3.
  // addAdjacency(adjacency, 1, {2, 5, 5, 3});
  // addAdjacency(adjacency, 2, {1, 5, 3, 7, 4, 9, 5, 4});
  // addAdjacency(adjacency, 3, {2, 7, 5, 2});
  // addAdjacency(adjacency, 4, {2, 9, 5, 1});
  // addAdjacency(adjacency, 5, {1, 3, 2, 4, 3, 2, 4, 1});

  // Example graph with only HACC machine
  //
  // addAdjacency(adjacency, 0, {11, 2});
  // addAdjacency(adjacency, 1, {14, 300, 12, 32});
  // addAdjacency(adjacency, 2, {14, 300, 12, 32});
  // addAdjacency(adjacency, 3, {12, 32, 14, 300});
  // addAdjacency(adjacency, 4, {12, 32, 14, 300});
  // addAdjacency(adjacency, 5, {13, 32, 14, 300});
  // addAdjacency(adjacency, 6, {13, 32, 14, 300});
  // addAdjacency(adjacency, 7, {13, 32, 14, 300});
  // addAdjacency(adjacency, 8, {13, 32, 14, 300});
  // addAdjacency(adjacency, 9, {12, 32, 10, 18});
  // addAdjacency(adjacency, 10, {13, 32, 9, 18});
  // addAdjacency(adjacency, 11, {12, 2, 0, 2});
  // addAdjacency(adjacency, 12, {4, 32, 3, 32, 9, 32, 11, 2, 1, 32, 2, 32});
  // addAdjacency(adjacency, 13, {10, 32, 7, 32, 8, 32, 6, 32, 5, 32});
  // addAdjacency(
  //     adjacency, 14,
  //     {1, 300, 4, 300, 5, 300, 2, 300, 6, 300, 8, 300, 3, 300, 7, 300});

  // Exmaple graph with HACC machine and V100 machine
  addAdjacency(adjacency, 0, {7, 2, 20, 2});
  addAdjacency(adjacency, 1, {8, 16, 2, 50, 3, 50, 4, 50});
  addAdjacency(adjacency, 2, {4, 50, 3, 50, 8, 16, 1, 50});
  addAdjacency(adjacency, 3, {4, 50, 2, 50, 1, 50, 8, 16});
  addAdjacency(adjacency, 4, {8, 16, 3, 50, 2, 50, 1, 50});
  addAdjacency(adjacency, 5, {6, 18, 8, 16});
  addAdjacency(adjacency, 6, {5, 18, 9, 16});
  addAdjacency(adjacency, 7, {0, 2, 8, 2});
  addAdjacency(adjacency, 8, {4, 16, 7, 2, 1, 16, 2, 16, 5, 16, 3, 16});
  addAdjacency(adjacency, 9, {6, 16});
  addAdjacency(adjacency, 10, {23, 300, 21, 32});
  addAdjacency(adjacency, 11, {21, 32, 23, 300});
  addAdjacency(adjacency, 12, {21, 32, 23, 300});
  addAdjacency(adjacency, 13, {21, 32, 23, 300});
  addAdjacency(adjacency, 14, {22, 32, 23, 300});
  addAdjacency(adjacency, 15, {22, 32, 23, 300});
  addAdjacency(adjacency, 16, {22, 32, 23, 300});
  addAdjacency(adjacency, 17, {22, 32, 23, 300});
  addAdjacency(adjacency, 18, {19, 18, 21, 32});
  addAdjacency(adjacency, 19, {22, 32, 18, 18});
  addAdjacency(adjacency, 20, {0, 2, 21, 2});
  addAdjacency(adjacency, 21, {13, 32, 20, 2, 11, 32, 18, 32, 12, 32, 10, 32});
  addAdjacency(adjacency, 22, {19, 32, 17, 32, 16, 32, 15, 32, 14, 32});
  addAdjacency(
      adjacency, 23,
      {16, 300, 10, 300, 11, 300, 13, 300, 15, 300, 12, 300, 17, 300, 14, 300});

  return adjacency;
}

//
// Main entry point for testing the weighted A* algorithm.
//
int main(int argc, char *argv[]) {

  int src_path = std::stoi(argv[1]);
  int dest_path = std::stoi(argv[2]);

  int num_paths = std::stoi(argv[3]);

  AstarSearch::Adjacency adjacency = createAdjacency();
  // Create an A* search instance from source node 1 to destination node 4.
  AstarSearch search(adjacency, src_path, dest_path);
  std::cout << "Printing all paths from lowest maximum weight to highest...\n";

  for (int i = 0; i < num_paths; ++i) {
    std::vector<int> path = search.nextShortestPath();
    if (path.empty())
      break;
    // Print the current path.
    for (int node : path) {
      std::cout << node << " ";
    }
    std::cout << "\n";
  }
  return 0;
}