maximum-weight-matching/cpp/mwmatching.h

/**
 * Algorithm for finding a maximum weight matching in general graphs.
 */

#ifndef MWMATCHING_H_
#define MWMATCHING_H_

#include <algorithm>
#include <cassert>
#include <cmath>
#include <cstddef>
#include <deque>
#include <limits>
#include <list>
#include <stack>
#include <stdexcept>
#include <tuple>
#include <utility>
#include <vector>


namespace mwmatching {

/* **************************************************
 * **        public definitions                    **
 * ************************************************** */

/** Type representing the unique ID of a vertex. */
using VertexId = unsigned int;

/** Type representing a pair of vertices. */
using VertexPair = std::pair<VertexId, VertexId>;


/** Type representing a weighted edge.
 *
 * @tparam WeightType  Numeric type used to represent edge weights.
 */
template <typename WeightType>
struct Edge
{
    static_assert(std::numeric_limits<WeightType>::is_specialized,
                  "Edge weight must be a numeric type");

    /** Incident vertices. */
    VertexPair vt;

    /** Edge weight. */
    WeightType weight;

    Edge(VertexPair vt, WeightType w)
        : vt(vt), weight(w)
    { }

    Edge(VertexId x, VertexId y, WeightType w)
        : vt(x, y), weight(w)
    { }
};


namespace impl {

/* **************************************************
 * **        private definitions                   **
 * ************************************************** */

/** Value used to mark an invalid or undefined vertex. */
constexpr VertexId NO_VERTEX = std::numeric_limits<VertexId>::max();


/** Top-level blossoms may be labeled "S" or "T" or unlabeled. */
enum BlossomLabel { LABEL_NONE = 0, LABEL_S = 1, LABEL_T = 2 };


/* **************************************************
 * **        private helper functions              **
 * ************************************************** */

/** Return a pair of vertices in flipped order. */
inline VertexPair flip_vertex_pair(const VertexPair& vt)
{
    return std::make_pair(vt.second, vt.first);
}


/**
 * Check that the input is a valid graph.
 *
 * The graph may not have self-edges or multiple edges between
 * the same pair of vertices. Edge weights must be within a valid range.
 *
 * This function takes time O(m * log(m)).
 *
 * @param  edges  Vector of weighted edges defining the graph.
 * @throw  std::invalid_argument  If the input graph is not valid.
 */
template <typename WeightType>
void check_input_graph(const std::vector<Edge<WeightType>>& edges)
{
    const VertexId max_num_vertex = std::numeric_limits<VertexId>::max();

    for (const Edge<WeightType>& edge : edges) {

        // Check that vertex IDs are valid.
        if ((edge.vt.first >= max_num_vertex)
                || (edge.vt.second >= max_num_vertex)) {
            throw std::invalid_argument("Vertex ID out of range");
        }

        // Check that edge weight is a finite number.
        if (! std::numeric_limits<WeightType>::is_integer) {
            if (! std::isfinite(edge.weight)) {
                throw std::invalid_argument(
                    "Edge weights must be finite numbers");
            }
        }

        // Check that edge weight will not cause overflow.
        if (edge.weight > std::numeric_limits<WeightType>::max() / 4) {
            throw std::invalid_argument(
                "Edge weight exceeds maximum supported value");
        }
    }

    // Check that the graph has no self-edges.
    for (const Edge<WeightType>& edge : edges) {
        if (edge.vt.first == edge.vt.second) {
            throw std::invalid_argument("Self-edges are not supported");
        }
    }

    // Check that the graph does not contain duplicate edges.
    std::vector<VertexPair> edge_endpoints;
    edge_endpoints.reserve(edges.size());
    for (const Edge<WeightType>& edge : edges) {
        VertexPair vt = edge.vt;
        if (vt.first > vt.second) {
            std::swap(vt.first, vt.second);
        }
        edge_endpoints.push_back(vt);
    }

    std::sort(edge_endpoints.begin(), edge_endpoints.end());
    if (std::unique(edge_endpoints.begin(), edge_endpoints.end())
            != edge_endpoints.end()) {
        throw std::invalid_argument("Duplicate edges are not supported");
    }
}


/* **************************************************
 * **        struct Graph                          **
 * ************************************************** */

/**
 * Representation of the input graph.
 */
template <typename WeightType>
struct Graph
{
    using EdgeT = Edge<WeightType>;

    /** Graph edges. */
    const std::vector<EdgeT> edges;

    /** Number of vertices. */
    const VertexId num_vertex;

    /** For each vertex, a vector of pointers to its incident edges. */
    const std::vector<std::vector<const EdgeT*>> adjacent_edges;

    /**
     * Initialize the graph representation and prepare adjacent edge lists.
     *
     * This function takes time O(n + m).
     */
    explicit Graph(const std::vector<EdgeT>& edges_in)
      : edges(remove_negative_weight_edges(edges_in)),
        num_vertex(count_num_vertex(edges)),
        adjacent_edges(build_adjacent_edges(edges, num_vertex))
    { }

    // Prevent copying.
    Graph(const Graph&) = delete;
    Graph& operator=(const Graph&) = delete;

    /**
     * Copy edges and remove edges with negative weight.
     *
     * This function takes time O(m).
     *
     * @param  edges  Vector of weighted edges defining the graph.
     * @return Vector of edges with non-negative weight.
     */
    static std::vector<EdgeT> remove_negative_weight_edges(
        const std::vector<EdgeT>& edges_in)
    {
        std::vector<EdgeT> real_edges;
        real_edges.reserve(edges_in.size());

        for (const Edge<WeightType>& edge : edges_in) {
            if (edge.weight >= 0) {
                real_edges.push_back(edge);
            }
        }

        return real_edges;
    }

    /** Count the number of vertices in the graph. */
    static VertexId count_num_vertex(const std::vector<EdgeT>& edges)
    {
        VertexId num_vertex = 0;
        for (const Edge<WeightType>& edge : edges) {
            VertexId m = std::max(edge.vt.first, edge.vt.second);
            assert(m < std::numeric_limits<VertexId>::max());
            num_vertex = std::max(num_vertex, m + 1);
        }

        return num_vertex;
    }

    /**
     * Build adjacency lists for a graph.
     *
     * Note that the adjacency lists contain pointers to edge instances,
     * and therefore become invalid if the input edge vector is modified
     * in any way that invalidates its iterators.
     *
     * This function takes time O(m).
     *
     * @param  num_vertex  Number of vertices in the graph.
     * @param  edges       List of edges in the graph.
     * @return Vector of incident edges for each vertex.
     */
    static std::vector<std::vector<const EdgeT*>> build_adjacent_edges(
        const std::vector<EdgeT>& edges,
        VertexId num_vertex)
    {
        // Count the number of incident edges per vertex.
        std::vector<VertexId> vertex_degree(num_vertex);
        for (const EdgeT& edge : edges) {
            assert(edge.vt.first < num_vertex);
            assert(edge.vt.second < num_vertex);
            vertex_degree[edge.vt.first]++;
            vertex_degree[edge.vt.second]++;
        }

        // Build adjacency lists.
        std::vector<std::vector<const EdgeT*>> adjacent_edges(num_vertex);
        for (VertexId i = 0; i < num_vertex; ++i) {
            adjacent_edges[i].reserve(vertex_degree[i]);
        }
        for (const EdgeT& edge : edges) {
            adjacent_edges[edge.vt.first].push_back(&edge);
            adjacent_edges[edge.vt.second].push_back(&edge);
        }

        return adjacent_edges;
    }
};


/* **************************************************
 * **        struct Blossom                        **
 * ************************************************** */

// Forward declaration.
template <typename WeightType> struct NonTrivialBlossom;


/**
 * Represents a blossom.
 *
 * A blossom is an odd-length alternating cycle over sub-blossoms.
 * An alternating path consists of alternating matched and unmatched edges.
 * An alternating cycle is an alternating path that starts and ends in
 * the same sub-blossom.
 *
 * A single vertex by itself is also a blossom: a "trivial blossom".
 *
 * An instance of this class represents either a trivial blossom,
 * or a non-trivial blossom which consists of multiple sub-blossoms.
 *
 * Blossoms are recursive structures: A non-trivial blossoms contains
 * sub-blossoms, which may themselves contain sub-blossoms etc.
 *
 * Each blossom contains exactly one vertex that is not matched to another
 * vertex in the same blossom. This is the "base vertex" of the blossom.
 */
template <typename WeightType>
struct Blossom
{
    /** Parent of this blossom, or "nullptr" if this blossom is top-level. */
    NonTrivialBlossom<WeightType>* parent;

    /** Index of the base vertex of this blossom. */
    VertexId base_vertex;

    /** Label S or T if this is a top-level blossom in an alternating tree. */
    BlossomLabel label;

    /** True if this is an instance of NonTrivialBlossom. */
    const bool is_nontrivial_blossom;

    /** Optional edge that attaches this blossom to the alternating tree. */
    VertexPair tree_edge;

    /**
     * In case of a top-level S-blossom, "best_edge" is the least-slack edge
     * that links to a different S-blossom, or "nullptr" if no such edge
     * has been found.
     */
    const Edge<WeightType>* best_edge;

protected:
    /** Initialize base class. */
    Blossom(VertexId base_vertex, bool is_nontrivial_blossom)
      : parent(nullptr),
        base_vertex(base_vertex),
        label(LABEL_NONE),
        is_nontrivial_blossom(is_nontrivial_blossom),
        best_edge(nullptr)
    { }

public:
    /** Initialize a trivial (single-vertex) blossom. */
    explicit Blossom(VertexId base_vertex)
      : Blossom(base_vertex, false)
    { }

    /**
     * Cast this Blossom instance to NonTrivialBlossom if possible,
     * otherwise return "nullptr".
     */
    NonTrivialBlossom<WeightType>* nontrivial()
    {
        return (is_nontrivial_blossom ?
                    static_cast<NonTrivialBlossom<WeightType>*>(this)
                    : nullptr);
    }

    const NonTrivialBlossom<WeightType>* nontrivial() const
    {
        return (is_nontrivial_blossom ?
                    static_cast<const NonTrivialBlossom<WeightType>*>(this)
                    : nullptr);
    }
};


/* **************************************************
 * **        struct NonTrivialBlossom              **
 * ************************************************** */

/**
 * Represents a non-trivial blossom.
 *
 * A non-trivial blossom is a blossom that contains multiple sub-blossoms
 * (at least 3 sub-blossoms, since all blossoms have odd length).
 *
 * Non-trivial blossoms maintain a list of their sub-blossoms and the edges
 * between their subblossoms.
 *
 * Unlike trivial blossoms, each non-trivial blossom is associated with
 * a variable in the dual LPP problem.
 *
 * Non-trivial blossoms are created and destroyed by the matching algorithm.
 * This implies that not every odd-length alternating cycle is a blossom;
 * it only becomes a blossom through an explicit action of the algorithm.
 * An existing blossom may change when the matching is augmented along
 * a path that runs through the blossom.
 */
template <typename WeightType>
struct NonTrivialBlossom : public Blossom<WeightType>
{
    struct SubBlossom {
        /** Pointer to sub-blossom. */
        Blossom<WeightType>* blossom;

        /**
         * Ordered pair of vertices (x, y) where "x" is a vertex in "blossom"
         * and "y" is a vertex in the next sub-blossom.
         */
        VertexPair edge;
    };

    /**
     * List of sub-blossoms.
     * Ordered by their appearance in the alternating cycle.
     * First blossom in the list is the start/end of the alternating cycle
     * and contains the base vertex.
     */
    std::list<SubBlossom> subblossoms;

    /** Dual LPP variable for this blossom. */
    WeightType dual_var;

    /**
     * In case of a top-level S-blossom, "best_edge_set" is a list of
     * least-slack edges between this blossom and other S-blossoms.
     */
    std::list<const Edge<WeightType>*> best_edge_set;

    /** Initialize a non-trivial blossom. */
    NonTrivialBlossom(
            const std::vector<Blossom<WeightType>*>& subblossoms,
            const std::deque<VertexPair>& edges)
      : Blossom<WeightType>(subblossoms.front()->base_vertex, true),
        dual_var(0)
    {
        assert(subblossoms.size() == edges.size());
        assert(subblossoms.size() % 2 == 1);
        assert(subblossoms.size() >= 3);

        auto blossom_it = subblossoms.begin();
        auto blossom_end = subblossoms.end();
        auto edge_it = edges.begin();
        while (blossom_it != blossom_end) {
            this->subblossoms.emplace_back();
            this->subblossoms.back().blossom = *blossom_it;
            this->subblossoms.back().edge = *edge_it;
            ++blossom_it;
            ++edge_it;
        }
    }

    /** Find the position of the specified subblossom. */
    std::pair<VertexId, typename std::list<SubBlossom>::iterator>
    find_subblossom(Blossom<WeightType>* subblossom)
    {
        VertexId pos = 0;
        auto it = subblossoms.begin();
        while (it->blossom != subblossom) {
            ++it;
            ++pos;
            assert(it != subblossoms.end());
        }
        return make_pair(pos, it);
    }
};


/** Call a function for every vertex inside the specified blossom. */
template <typename WeightType, typename Func>
inline void for_vertices_in_blossom(const Blossom<WeightType>* blossom, Func func)
{
    const NonTrivialBlossom<WeightType>* ntb = blossom->nontrivial();
    if (ntb) {
        // Visit all vertices in the non-trivial blossom.
        // Use an explicit stack to avoid deep call chains.
        std::vector<const NonTrivialBlossom<WeightType>*> stack;
        stack.push_back(ntb);

        while (! stack.empty()) {
            const NonTrivialBlossom<WeightType>* b = stack.back();
            stack.pop_back();

            for (const auto& sub : b->subblossoms) {
                ntb = sub.blossom->nontrivial();
                if (ntb) {
                    stack.push_back(ntb);
                } else {
                    func(sub.blossom->base_vertex);
                }
            }
        }

    } else {
        // A trivial blossom contains just one vertex.
        func(blossom->base_vertex);
    }
}


/**
 * Represents a list of edges forming an alternating path or
 * an alternating cycle.
 *
 * The path is defined over top-level blossoms; it skips parts of the path
 * that are internal to blossoms. Vertex pairs are oriented to match the
 * direction of the path.
 */
struct AlternatingPath
{
    std::deque<VertexPair> edges;
};


/* **************************************************
 * **        struct MatchingContext                **
 * ************************************************** */

/**
 * This class holds all data used by the matching algorithm.
 *
 * It contains the input graph, a partial solution, and several
 * auxiliary data structures.
 */
template <typename WeightType>
class MatchingContext
{
public:

    using EdgeT = Edge<WeightType>;
    using BlossomT = Blossom<WeightType>;
    using NonTrivialBlossomT = NonTrivialBlossom<WeightType>;

    /** Specification of a delta step. */
    struct DeltaStep
    {
        /** Type of delta step: 1, 2, 3 or 4. */
        int kind;

        /** Delta value. */
        WeightType value;

        /** Edge on which the minimum delta occurs (for delta type 2 or 3). */
        VertexPair edge;

        /** Blossom on which the minimum delta occurs (for delta type 4). */
        NonTrivialBlossomT* blossom;
    };

    /** Scale integer edge weights to enable integer calculations. */
    static constexpr WeightType weight_factor =
        std::numeric_limits<WeightType>::is_integer ? 2 : 1;

    /** Input graph. */
    const Graph<WeightType> graph;

    /**
     * Current state of the matching.
     *
     * vertex_mate[x] == y          if vertex "x" is matched to vertex "y".
     * vertex_mate[x] == NO_VERTEX  if vertex "x" is unmatched.
     */
    std::vector<VertexId> vertex_mate;

    /**
     * Each vertex is associated with a trivial blossom.
     *
     * "trivial_blossom[x]" is the trivial blossom that contains only
     * vertex "x".
     */
    std::vector<BlossomT> trivial_blossom;

    /**
     * Non-trivial blossoms may be created and destroyed during
     * the course of the algorithm.
     */
// NOTE - this MUST be a list, because we delete items from it while keeping pointers to other items
    std::list<NonTrivialBlossomT> nontrivial_blossom;

    /**
     * Every vertex is contained in exactly one top-level blossom
     * (possibly the trivial blossom that contains just that vertex).
     *
     * "vertex_top_blossom[x]" is the top-level blossom that contains
     * vertex "x".
     */
    std::vector<BlossomT*> vertex_top_blossom;

    /**
     * Every vertex has a variable in the dual LPP.
     *
     * "vertex_dual[x]" is the dual variable of vertex "x".
     */
    std::vector<WeightType> vertex_dual;

    /**
     * In case of T-vertex or unlabeled vertex "x",
     * "vertex_best_edge[x]" is the least-slack edge to any S-vertex,
     * or "nullptr" if no such edge has been found.
     */
    std::vector<const EdgeT*> vertex_best_edge;

    /** Queue of S-vertices to be scanned. */
    std::deque<VertexId> queue;

    /** Markers placed while tracing an alternating path. */
    std::vector<bool> vertex_marker;

    /** Vertices marked while tracing an alternating path. */
    std::vector<VertexId> marked_vertex;

    /**
     * Initialize the matching algorithm.
     *
     * This function takes time O(n + m).
     */
    explicit MatchingContext(const std::vector<EdgeT>& edges_in)
      : graph(edges_in)
    {
        // Initially all vertices are unmatched.
        vertex_mate.resize(graph.num_vertex, NO_VERTEX);

        // Create a trivial blossom for each vertex.
        trivial_blossom.reserve(graph.num_vertex);
        for (VertexId x = 0; x < graph.num_vertex; ++x) {
            trivial_blossom.emplace_back(x);
        }

        // Initially all vertices are trivial top-level blossoms.
        vertex_top_blossom.reserve(graph.num_vertex);
        for (VertexId x = 0; x < graph.num_vertex; ++x) {
            vertex_top_blossom.push_back(&trivial_blossom[x]);
        }

        // Vertex duals are initialized to half the maximum edge weight.
        WeightType max_weight = 0;
        for (const EdgeT& edge : graph.edges) {
            max_weight = std::max(max_weight, edge.weight);
        }
        WeightType init_vertex_dual = max_weight * (weight_factor / 2);
        vertex_dual.resize(graph.num_vertex, init_vertex_dual);

        // Initialize "vertex_best_edge".
        vertex_best_edge.resize(graph.num_vertex, nullptr);

        // Allocate temporary arrays for path tracing.
        vertex_marker.resize(graph.num_vertex);
        marked_vertex.reserve(graph.num_vertex);
    }

    // Prevent copying.
    MatchingContext(const MatchingContext&) = delete;
    MatchingContext& operator=(const MatchingContext&) = delete;

    /* **********  General support routines:  ********** */

    /** Calculate edge slack. */
    WeightType edge_slack(const EdgeT& edge) const
    {
        VertexId x = edge.vt.first;
        VertexId y = edge.vt.second;
        assert(vertex_top_blossom[x] != vertex_top_blossom[y]);
        return vertex_dual[x] + vertex_dual[y] - weight_factor * edge.weight;
    }

    /*
     * Least-slack edge tracking:
     *
     * To calculate delta steps, the matching algorithm needs to find
     *  - the least-slack edge between any S-vertex and an unlabeled vertex;
     *  - the least-slack edge between any pair of top-level S-blossoms.
     *
     * For each unlabeled vertex and each T-vertex, we keep track of the
     * least-slack edge to any S-vertex. Tracking for unlabeled vertices
     * serves to provide the least-slack edge for the delta step.
     * Tracking for T-vertices is done because such vertices can turn into
     * unlabeled vertices if they are part of a T-blossom that gets expanded.
     *
     * For each top-level S-blossom, we keep track of the least-slack edge
     * to any S-vertex not in the same blossom.
     *
     * Furthermore, for each top-level S-blossom, we keep a list of
     * least-slack edges to other top-level S-blossoms. For any pair of
     * top-level S-blossoms, the least-slack edge between them is contained
     * in the edge list of at least one of the blossoms. An edge list may
     * contain multiple edges to the same S-blossom. Such redundant edges are
     * pruned during blossom merging to limit the number of tracked edges.
     *
     * Note: For a given vertex or blossom, the identity of the least-slack
     * edge to any S-blossom remains unchanged during a delta step.
     * Although the delta step changes edge slacks, it changes the slack
     * of every edge to an S-vertex by the same amount. Therefore the edge
     * that had least slack before the delta step, will still have least slack
     * after the delta step.
     */

    /**
     * Reset least-slack edge tracking.
     *
     * This function takes time O(n + m).
     */
    void lset_reset()
    {
        for (VertexId x = 0; x < graph.num_vertex; ++x) {
            vertex_best_edge[x] = nullptr;
        }

        for (BlossomT& blossom : trivial_blossom) {
            blossom.best_edge = nullptr;
        }

        for (NonTrivialBlossomT& blossom : nontrivial_blossom) {
            blossom.best_edge = nullptr;
            blossom.best_edge_set.clear();
        }
    }

    /**
     * Add edge "e" from an S-vertex to unlabeled vertex or T-vertex "y".
     *
     * This function takes time O(1) per call.
     * This function is called O(m) times per stage.
     */
    void lset_add_vertex_edge(VertexId y, const EdgeT* edge, WeightType slack)
    {
        const EdgeT* cur_best_edge = vertex_best_edge[y];
        if ((cur_best_edge == nullptr)
                || (slack < edge_slack(*cur_best_edge))) {
            vertex_best_edge[y] = edge;
        }
    }

    /**
     * Return the index and slack of the least-slack edge between
     * any S-vertex and unlabeled vertex.
     *
     * This function takes time O(n) per call.
     * This function takes total time O(n**2) per stage.
     *
     * @return Pair (edge, slack) if there is a least-slack edge,
     *         or (nullptr, 0) if there is no suitable edge.
     */
    std::pair<const EdgeT*, WeightType> lset_get_best_vertex_edge()
    {
        const EdgeT* best_edge = nullptr;
        WeightType best_slack = 0;

        for (VertexId x = 0; x < graph.num_vertex; ++x) {
            if (vertex_top_blossom[x]->label == LABEL_NONE) {
                const EdgeT* edge = vertex_best_edge[x];
                if (edge != nullptr) {
                    WeightType slack = edge_slack(*edge);
                    if ((best_edge == nullptr) || (slack < best_slack)) {
                        best_edge = edge;
                        best_slack = slack;
                    }
                }
            }
        }

        return std::make_pair(best_edge, best_slack);
    }

    /** Start tracking edges for a new S-blossom. */
    void lset_new_blossom(BlossomT* blossom)
    {
        assert(blossom->best_edge == nullptr);
        assert((blossom->nontrivial() == nullptr)
               || blossom->nontrivial()->best_edge_set.empty());
    }

    /**
     * Add edge "e" between the specified S-blossom and another S-blossom.
     *
     * This function takes time O(1) per call.
     * This function is called O(m) times per stage.
     */
    void lset_add_blossom_edge(BlossomT* blossom,
                               const EdgeT* edge,
                               WeightType slack)
    {
        const EdgeT* cur_best_edge = blossom->best_edge;
        if ((cur_best_edge == nullptr)
                || (slack < edge_slack(*cur_best_edge))) {
            blossom->best_edge = edge;
        }

        NonTrivialBlossomT* ntb = blossom->nontrivial();
        if (ntb) {
            ntb->best_edge_set.push_back(edge);
        }
    }

    /**
     * Update least-slack edge tracking after merging sub-blossoms
     * into a new S-blossom.
     *
     * This function takes total time O(n**2) per stage.
     */
    void lset_merge_blossoms(NonTrivialBlossomT* blossom)
    {
        assert(blossom->best_edge == nullptr);
        assert(blossom->best_edge_set.empty());

        // Collect edges from the sub-blossoms that used to be S-blossoms.
        for (auto& subblossom_item : blossom->subblossoms) {
            BlossomT* sub = subblossom_item.blossom;
            if (sub->label == LABEL_S) {
                NonTrivialBlossomT* ntb = sub->nontrivial();
                if (ntb) {
                    // Take least-slack edge set from this subblossom.
                    blossom->best_edge_set.splice(
                        blossom->best_edge_set.end(),
                        ntb->best_edge_set);
                } else {
                    // Trivial blossoms don't maintain a least-slack edge set.
                    // Just consider all incident edges.
                    for (const EdgeT* edge : graph.adjacent_edges[sub->base_vertex]) {
                        // Only take edges between different S-blossoms.
                        VertexId x = edge->vt.first;
                        VertexId y = edge->vt.second;
                        BlossomT* bx = vertex_top_blossom[x];
                        BlossomT* by = vertex_top_blossom[y];
                        if ((bx != by)
                                && (bx->label == LABEL_S)
                                && (by->label == LABEL_S)) {
                            blossom->best_edge_set.push_back(edge);
                        }
                    }
                }
            }
        }

        // Build a temporary array holding the least-slack edge index to
        // each top-level S-blossom. This array is indexed by the base vertex
        // of the blossoms.
        std::vector<std::pair<const EdgeT*, WeightType>>
            best_edge_to_blossom(graph.num_vertex);

        for (const EdgeT* edge : blossom->best_edge_set) {
            BlossomT* bx = vertex_top_blossom[edge->vt.first];
            BlossomT* by = vertex_top_blossom[edge->vt.second];
            assert(bx == blossom || by == blossom);

            // Ignore internal edges.
            if (bx != by) {
                bx = (bx == blossom) ? by : bx;

                // Only consider edges to S-blossoms.
                if (bx->label == LABEL_S) {

                    // Keep only the least-slack edge to blossom "bx".
                    WeightType slack = edge_slack(*edge);
                    VertexId bx_base = bx->base_vertex;
                    auto& best_edge_item = best_edge_to_blossom[bx_base];
                    if ((best_edge_item.first == nullptr)
                            || (slack < best_edge_item.second)) {
                        best_edge_item.first = edge;
                        best_edge_item.second = slack;
                    }
                }
            }
        };

        // Rebuild a compact list of least-slack edges.
        // Also find the overall least-slack edge to any other S-blossom.
        WeightType best_slack = 0;
        blossom->best_edge_set.clear();
        for (auto& best_edge_item : best_edge_to_blossom) {
            const EdgeT* edge = best_edge_item.first;
            if (edge != nullptr) {
                blossom->best_edge_set.push_back(edge);
                WeightType slack = best_edge_item.second;
                if ((blossom->best_edge == nullptr) || (slack < best_slack)) {
                    blossom->best_edge = edge;
                    best_slack = slack;
                }
            }
        }
    }

    /**
     * Return the index and slack of the least-slack edge between
     * any pair of top-level S-blossoms.
     *
     * This function takes time O(n) per call.
     * This function takes total time O(n**2) per stage.
     *
     * @return Pair (edge, slack) if there is a least-slack edge,
     *         or (nullptr, 0) if there is no suitable edge.
     */
    std::pair<const EdgeT*, WeightType> lset_get_best_blossom_edge()
    {
        const EdgeT* best_edge = nullptr;
        WeightType best_slack = 0;

        auto consider_blossom =
            [this,&best_edge,&best_slack](BlossomT* blossom)
        {
            if ((blossom->parent == nullptr) && (blossom->label == LABEL_S)) {
                const EdgeT* edge = blossom->best_edge;
                if (edge != nullptr) {
                    WeightType slack = edge_slack(*edge);
                    if ((best_edge == nullptr) || (slack < best_slack)) {
                        best_edge = edge;
                        best_slack = slack;
                    }
                }
            }
        };

        for (BlossomT& blossom : trivial_blossom) {
            consider_blossom(&blossom);
        }
        for (BlossomT& blossom : nontrivial_blossom) {
            consider_blossom(&blossom);
        }

        return std::make_pair(best_edge, best_slack);
    }

    /* **********  Creating and expanding blossoms:  ********** */

    /**
     * Trace back through the alternating trees from vertices "x" and "y".
     *
     * If both vertices are part of the same alternating tree, this function
     * discovers a new blossom. In this case it returns an alternating path
     * through the blossom that starts and ends in the same sub-blossom.
     *
     * If the vertices are part of different alternating trees, this function
     * discovers an augmenting path. In this case it returns an alternating
     * path that starts and ends in an unmatched vertex.
     *
     * This function takes time O(k) to discover a blossom, where "k" is the
     * number of sub-blossoms, or time O(n) to discover an augmenting path.
     */
    AlternatingPath trace_alternating_paths(VertexId x, VertexId y)
    {
        assert(vertex_top_blossom[x] != vertex_top_blossom[y]);

        // Initialize a path containing only the edge (x, y).
        AlternatingPath path;
        path.edges.emplace_back(x, y);

        // "first_common" is the first common ancestor of "x" and "y"
        // in the alternating tree, or "nullptr" if no common ancestor
        // has been found.
        BlossomT* first_common = nullptr;

        // Alternate between tracing the path from "x" and the path from "y".
        // This ensures that the search time is bounded by the size of any
        // newly found blossom.
        while ((x != NO_VERTEX) || (y != NO_VERTEX)) {

            // Trace path from vertex "x".
            if (x != NO_VERTEX) {

                // Stop if we found a common ancestor.
                BlossomT* bx = vertex_top_blossom[x];
                if (vertex_marker[bx->base_vertex]) {
                    first_common = bx;
                    break;
                }

                // Mark blossom as potential common ancestor.
                vertex_marker[bx->base_vertex] = true;
                marked_vertex.push_back(bx->base_vertex);

                // Trace back to the parent in the alternating tree.
                x = bx->tree_edge.first;
                if (x != NO_VERTEX) {
                    path.edges.push_front(bx->tree_edge);
                }
            }

            // Trace path from vertex "y".
            if (y != NO_VERTEX) {

                // Stop if we found a common ancestor.
                BlossomT* by = vertex_top_blossom[y];
                if (vertex_marker[by->base_vertex]) {
                    first_common = by;
                    break;
                }

                // Mark blossom as potential common ancestor.
                vertex_marker[by->base_vertex] = true;
                marked_vertex.push_back(by->base_vertex);

                // Trace back to the parent in the alternating tree.
                y = by->tree_edge.first;
                if (y != NO_VERTEX) {
                    path.edges.emplace_back(by->tree_edge.second, y);
                }
            }
        }

        // Remove all markers we placed.
        for (VertexId k : marked_vertex) {
            vertex_marker[k] = false;
        }
        marked_vertex.clear();

        // If we found a common ancestor, trim the paths so they end there.
        if (first_common) {
            assert(first_common->label == LABEL_S);
            while (vertex_top_blossom[path.edges.front().first]
                   != first_common) {
                path.edges.pop_front();
            }
            while (vertex_top_blossom[path.edges.back().second]
                   != first_common) {
                path.edges.pop_back();
            }
        }

        // Any alternating path between S-blossoms must have odd length.
        assert(path.edges.size() % 2 == 1);

        return path;
    }

    /**
     * Create a new blossom from an alternating cycle.
     *
     * Assign label S to the new blossom.
     * Relabel all T-sub-blossoms as S and add their vertices to the queue.
     *
     * This function takes total time O(n**2) per stage.
     */
    void make_blossom(const AlternatingPath& path)
    {
        assert(path.edges.size() % 2 == 1);
        assert(path.edges.size() >= 3);

        // Construct the list of sub-blossoms.
        std::vector<BlossomT*> subblossoms;
        subblossoms.reserve(path.edges.size());
        for (VertexPair edge : path.edges) {
            subblossoms.push_back(vertex_top_blossom[edge.first]);
        }

        // Check that the path is cyclic.
        VertexId pos = 0;
        for (VertexPair edge : path.edges) {
            pos = (pos + 1) % subblossoms.size();
            assert(vertex_top_blossom[edge.second] == subblossoms[pos]);
        }

        // Create the new blossom object.
        nontrivial_blossom.emplace_back(subblossoms, path.edges);
        NonTrivialBlossomT* blossom = &nontrivial_blossom.back();

        // Link the subblossoms to the their new parent.
        for (BlossomT* sub : subblossoms) {
            sub->parent = blossom;
        }

        // Mark vertices as belonging to the new blossom.
        for_vertices_in_blossom(blossom, [this,blossom](VertexId x) {
            vertex_top_blossom[x] = blossom;
        });

        // Assign label S to the new blossom and link to the alternating tree.
        assert(subblossoms.front()->label == LABEL_S);
        blossom->label = LABEL_S;
        blossom->tree_edge = subblossoms.front()->tree_edge;

        // Consider vertices inside former T-sub-blossoms which now
        // became S-vertices; add them to the queue.
        for (BlossomT* sub : subblossoms) {
            if (sub->label == LABEL_T) {
                for_vertices_in_blossom(sub,
                    [this](VertexId x) {
                        queue.push_back(x);
                    });
            }
        }

        // Merge least-slack edges for the S-sub-blossoms.
        lset_merge_blossoms(blossom);
    }

    /** Erase the specified non-trivial blossom. */
    void erase_nontrivial_blossom(NonTrivialBlossomT* blossom)
    {
        auto blossom_it = std::find_if(
            nontrivial_blossom.begin(),
            nontrivial_blossom.end(),
            [blossom](const NonTrivialBlossomT& b) {
                return (&b == blossom);
            });
        assert(blossom_it != nontrivial_blossom.end());
        nontrivial_blossom.erase(blossom_it);
    }

    /**
     * Expand the specified T-blossom.
     *
     * This function takes time O(n).
     */
    void expand_t_blossom(NonTrivialBlossomT* blossom)
    {
        assert(blossom->parent == nullptr);
        assert(blossom->label == LABEL_T);

        // Convert sub-blossoms into top-level blossoms.
        for (const auto& sub : blossom->subblossoms) {
            BlossomT* sub_blossom = sub.blossom;
            assert(sub_blossom->parent == blossom);
            assert(sub_blossom->label == LABEL_NONE);
            sub_blossom->parent = nullptr;
            for_vertices_in_blossom(sub_blossom,
                [this,sub_blossom](VertexId x) {
                    vertex_top_blossom[x] = sub_blossom;
                });
        }

        // The expanded blossom was part of an alternating tree.
        // We must now reconstruct the part of the alternating tree
        // that ran through this blossom by linking some of the sub-blossoms
        // into the tree.

        // Find the sub-blossom that was attached to the parent node
        // in the alternating tree.
        BlossomT* entry = vertex_top_blossom[blossom->tree_edge.second];

        // Assign label T to that blossom and link to the alternating tree.
        entry->label = LABEL_T;
        entry->tree_edge = blossom->tree_edge;

        // Find the position of this sub-blossom within the expanding blossom.
        auto subblossom_loc = blossom->find_subblossom(entry);
        VertexId entry_pos = subblossom_loc.first;
        auto entry_it = subblossom_loc.second;

        // Walk around the blossom from "entry" to the base
        // in an even number of steps.
        auto sub_it = entry_it;
        if (entry_pos % 2 == 0) {

            // Walk backward to the base.
            auto sub_begin = blossom->subblossoms.begin();
            while (sub_it != sub_begin) {
                // Assign label S to the next node on the path.
                --sub_it;
                assign_label_s(sub_it->edge.first);

                // Assign label T to the next node on the path.
                assert(sub_it != sub_begin);
                --sub_it;
                sub_it->blossom->label = LABEL_T;
                sub_it->blossom->tree_edge = flip_vertex_pair(sub_it->edge);
            }

        } else {

            // Walk forward to the base.
            auto sub_end = blossom->subblossoms.end();
            while (sub_it != sub_end) {
                // Assign label S to the next node on the path.
                assign_label_s(sub_it->edge.second);
                ++sub_it;

                // Assign label T to the next node on the path.
                // We may wrap past the end of the subblossom list.
                assert(sub_it != sub_end);
                VertexPair& tree_edge = sub_it->edge;
                ++sub_it;

                BlossomT *sub_blossom = (sub_it == sub_end) ?
                    blossom->subblossoms.front().blossom :
                    sub_it->blossom;
                sub_blossom->label = LABEL_T;
                sub_blossom->tree_edge = tree_edge;
            }
        }

        // Delete the expanded blossom.
        erase_nontrivial_blossom(blossom);
    }

    /**
     * Expand the specified unlabeled blossom.
     *
     * This function takes time O(n).
     */
    void expand_unlabeled_blossom(NonTrivialBlossomT* blossom)
    {
        assert(blossom->parent == nullptr);
        assert(blossom->label == LABEL_NONE);

        // Convert sub-blossoms into top-level blossoms.
        for (const auto& sub : blossom->subblossoms) {
            BlossomT* sub_blossom = sub.blossom;
            assert(sub_blossom->parent == blossom);
            assert(sub_blossom->label == LABEL_NONE);
            sub_blossom->parent = nullptr;
            for_vertices_in_blossom(sub_blossom,
                [this,sub_blossom](VertexId x) {
                    vertex_top_blossom[x] = sub_blossom;
                });
        }

        // Delete the expanded blossom.
        erase_nontrivial_blossom(blossom);
    }

    /* **********  Augmenting:  ********** */

    /**
     * Augment along an alternating path through the specified blossom,
     * from sub-blossom "entry" to the base vertex of the blossom.
     *
     * Modify the blossom to reflect that sub-blossom "entry" contains
     * the base vertex after augmenting.
     */
    void augment_blossom_rec(
            NonTrivialBlossomT* blossom,
            BlossomT* entry,
            std::stack<std::pair<NonTrivialBlossomT*, BlossomT*>>& rec_stack)
    {
        // Find the position of "entry" within "blossom".
        auto subblossom_loc = blossom->find_subblossom(entry);
        VertexId entry_pos = subblossom_loc.first;
        auto entry_it = subblossom_loc.second;

        // Walk around the blossom from "entry" to the base
        // in an even number of steps.
        auto sub_begin = blossom->subblossoms.begin();
        auto sub_end = blossom->subblossoms.end();
        auto sub_it = entry_it;
        while ((sub_it != sub_begin) && (sub_it != sub_end)) {
            VertexId x, y;
            BlossomT* bx;
            BlossomT* by;

            if (entry_pos % 2 == 0) {
                // Walk backward to the base.
                --sub_it;
                by = sub_it->blossom;
                assert(sub_it != sub_begin);
                --sub_it;
                bx = sub_it->blossom;
                std::tie(x, y) = sub_it->edge;
            } else {
                // Walk forward to the base.
                // We may wrap past the end of the subblossom list.
                ++sub_it;
                assert(sub_it != sub_end);
                std::tie(x, y) = sub_it->edge;
                bx = sub_it->blossom;
                ++sub_it;
                by = (sub_it == sub_end) ?
                    blossom->subblossoms.front().blossom :
                    sub_it->blossom;
            }

            // Pull this edge into the matching.
            vertex_mate[x] = y;
            vertex_mate[y] = x;

            // Augment through any non-trivial subblossoms touching this edge.
            NonTrivialBlossomT* bx_ntb = bx->nontrivial();
            if (bx_ntb != nullptr) {
                rec_stack.emplace(bx_ntb, &trivial_blossom[x]);
            }

            NonTrivialBlossomT* by_ntb = by->nontrivial();
            if (by_ntb != nullptr) {
                rec_stack.emplace(by_ntb, &trivial_blossom[y]);
            }
        }

        // Re-orient the blossom.
        if (entry_it != sub_begin) {
            // Rotate the subblossom list.
            blossom->subblossoms.splice(sub_begin,
                                        blossom->subblossoms,
                                        entry_it,
                                        sub_end);
        }

        // Update the base vertex.
        // We can pull the new base vertex from the entry sub-blossom
        // since its augmentation has already finished.
        blossom->base_vertex = entry->base_vertex;
    }

    /**
     * Augment along an alternating path through the specified blossom,
     * from sub-blossom "entry" to the base vertex of the blossom.
     *
     * Recursively handle sub-blossoms as needed.
     *
     * This function takes time O(n).
     */
    void augment_blossom(NonTrivialBlossomT* blossom, BlossomT* entry)
    {
        // Use an explicit stack to avoid deep recursion.
        std::stack<std::pair<NonTrivialBlossomT*, BlossomT*>> rec_stack;
        rec_stack.emplace(blossom, entry);

        while (!rec_stack.empty()) {
            NonTrivialBlossomT* outer_blossom;
            BlossomT* inner_entry;
            std::tie(outer_blossom, inner_entry) = rec_stack.top();

            NonTrivialBlossomT* inner_blossom = inner_entry->parent;
            assert(inner_blossom != nullptr);

            if (inner_blossom != outer_blossom) {
                // After augmenting "inner_blossom",
                // continue by augmenting its parent.
                rec_stack.top() = std::make_pair(outer_blossom,
                                                 inner_blossom);
            } else {
                // After augmenting "inner_blossom",
                // this entire "outer_blossom" will be finished.
                rec_stack.pop();
            }

            // Augment "inner_blossom".
            augment_blossom_rec(inner_blossom, inner_entry, rec_stack);
        }
    }

    /**
     * Augment the matching through the specified augmenting path.
     *
     * This function takes time O(n).
     */
    void augment_matching(const AlternatingPath& path)
    {
        // Check that the path starts and ends in an unmatched blossom.
        assert(path.edges.size() % 2 == 1);
        assert(vertex_mate[vertex_top_blossom[path.edges.front().first]->base_vertex] == NO_VERTEX);
        assert(vertex_mate[vertex_top_blossom[path.edges.back().second]->base_vertex] == NO_VERTEX);

        // Process the unmatched edges on the augmenting path.
        auto edge_it = path.edges.begin();
        auto edge_end = path.edges.end();
        while (edge_it != edge_end) {
            VertexId x = edge_it->first;
            VertexId y = edge_it->second;

            // Augment any non-trivial blossoms that touch this edge.
            BlossomT* bx = vertex_top_blossom[x];
            NonTrivialBlossomT* bx_ntb = bx->nontrivial();
            if (bx_ntb != nullptr) {
                augment_blossom(bx_ntb, &trivial_blossom[x]);
            }

            BlossomT* by = vertex_top_blossom[y];
            NonTrivialBlossomT* by_ntb = by->nontrivial();
            if (by_ntb != nullptr) {
                augment_blossom(by_ntb, &trivial_blossom[y]);
            }

            // Pull this edge into the matching.
            vertex_mate[x] = y;
            vertex_mate[y] = x;

            // Move forward through the augmenting path to
            // the next edge to be matched.
            ++edge_it;
            if (edge_it == edge_end) {
                break;
            }
            ++edge_it;
        }
    }

    /* **********  Labels and alternating tree:  ********** */

    /**
     * Assign label S to the unlabeled blossom that contains vertex "x".
     *
     * If vertex "x" is matched, it becomes attached to the alternating tree
     * via its matched edge. If vertex "x" is unmatched, it becomes the root
     * of an alternating tree.
     *
     * All vertices in the newly labeled blossom are added to the scan queue.
     *
     * @pre "x" is an unlabeled vertex, either unmatched or matched to
     *      a T-vertex via a tight edge.
     */
    void assign_label_s(VertexId x)
    {
        // Assign label S to the blossom that contains vertex "x".
        BlossomT* bx = vertex_top_blossom[x];
        assert(bx->label == LABEL_NONE);
        bx->label = LABEL_S;

        VertexId y = vertex_mate[x];
        if (y == NO_VERTEX) {
            // Vertex "x" is unmatched.
            // It must be either a top-level vertex or the base vertex of
            // a top-level blossom.
            assert(bx->base_vertex == x);

            // Mark the blossom as root of an alternating tree.
            bx->tree_edge = std::make_pair(NO_VERTEX, x);

        } else {
            // Vertex "x" is matched to T-vertex "y".
            assert(vertex_top_blossom[y]->label == LABEL_T);

            // Attach the blossom to the alternating tree via vertex "y".
            bx->tree_edge = std::make_pair(y, x);
        }

        // Start least-slack edge tracking for the S-blossom.
        lset_new_blossom(bx);

        // Add all vertices inside the newly labeled S-blossom to the queue.
        for_vertices_in_blossom(bx,
            [this](VertexId v) {
                queue.push_back(v);
            });
    }

    /**
     * Assign label T to the unlabeled blossom that contains vertex "y".
     *
     * Attach it to the alternating tree via edge (x, y).
     * Then immediately assign label S to the mate of vertex "y".
     *
     * Note that this function may expand blossoms that contain vertex "y".
     *
     * @pre "x" is an S-vertex.
     * @pre "y" is an unlabeled, matched vertex.
     * @pre There is a tight edge between vertices "x" and "y".
     */
    void assign_label_t(VertexId x, VertexId y)
    {
        assert(vertex_top_blossom[x]->label == LABEL_S);

        BlossomT* by = vertex_top_blossom[y];
        assert(by->label == LABEL_NONE);

        // If "y" is part of a zero-dual blossom, expand it.
        // This would otherwise likely happen through a zero-delta4 step,
        // so we can just do it now and avoid a substage.
        NonTrivialBlossomT* ntb = by->nontrivial();
        while (ntb != nullptr && ntb->dual_var == 0) {
            expand_unlabeled_blossom(ntb);
            by = vertex_top_blossom[y];
            assert(by->label == LABEL_NONE);
            ntb = by->nontrivial();
        }

        // Assign label T to the top-level blossom that contains vertex "y".
        by->label = LABEL_T;
        by->tree_edge = std::make_pair(x, y);

        // Assign label S to the blossom that is mated to the T-blossom.
        VertexId z = vertex_mate[by->base_vertex];
        assert(z != NO_VERTEX);
        assign_label_s(z);
    }

    /**
     * Add the edge between S-vertices "x" and "y".
     *
     * If the edge connects blossoms that are part of the same alternating
     * tree, this function creates a new S-blossom and returns false.
     *
     * If the edge connects two different alternating trees, an augmenting
     * path has been discovered. In this case the matching is augmented.
     *
     * @return True if the matching was augmented; otherwise false.
     */
    bool add_s_to_s_edge(VertexId x, VertexId y)
    {
        assert(vertex_top_blossom[x]->label == LABEL_S);
        assert(vertex_top_blossom[y]->label == LABEL_S);

        // Trace back through the alternating trees from "x" and "y".
        AlternatingPath path = trace_alternating_paths(x, y);

        // If the path is a cycle, create a new blossom.
        // Otherwise the path is an augmenting path.
        // Note that an alternating starts and ends in the same blossom,
        // but not necessarily in the same vertex within that blossom.
        VertexId p = path.edges.front().first;
        VertexId q = path.edges.back().second;
        if (vertex_top_blossom[p] == vertex_top_blossom[q]) {
            make_blossom(path);
            return false;
        } else {
            augment_matching(path);
            return true;
        }
    }

    /**
     * Scan queued S-vertices to expand the alternating trees.
     *
     * The scan proceeds until either an augmenting path is found,
     * or the queue of S-vertices becomes empty.
     * If an augmenting path is found, it is used to augment the matching.
     *
     * New blossoms may be created during the scan.
     *
     * @return True if the matching was augmented; otherwise false.
     */
    bool substage_scan()
    {
        // Process the queue of S-vertices to be scanned.
        // This loop runs through O(n) iterations per stage.
        while (! queue.empty()) {

            // Take one vertex from the queue.
            VertexId x = queue.front();
            queue.pop_front();

            assert(vertex_top_blossom[x]->label == LABEL_S);

            // Scan the edges that are incident on "x".
            // This loop runs through O(m) iterations per stage.
            for (const EdgeT* edge : graph.adjacent_edges[x]) {
                VertexId y = (edge->vt.first != x) ? edge->vt.first
                                                   : edge->vt.second;

                // Note: The top-level blossom of vertex "x" may change
                // during this loop, so we need to refresh it in each pass.
                BlossomT* bx = vertex_top_blossom[x];

                // Ignore edges that are internal to a blossom.
                if (bx == vertex_top_blossom[y]) {
                    continue;
                }

                BlossomLabel ylabel = vertex_top_blossom[y]->label;

                // Check whether this edge is tight (has zero slack).
                // Only tight edges may be part of an alternating tree.
                WeightType slack = edge_slack(*edge);
                if (slack <= 0) {
                    if (ylabel == LABEL_NONE) {
                        // Found a tight edge to an unlabeled blossom.
                        // Assign label T to the blossom that contains "y".
                        assign_label_t(x, y);
                    } else if (ylabel == LABEL_S) {
                        // Found a tight edge between two S-blossoms.
                        // Find either a new blossom or an augmenting path.
                        bool augmented = add_s_to_s_edge(x, y);
                        if (augmented) {
                            return true;
                        }
                    }
                } else if (ylabel == LABEL_S) {
                    // Found a non-tight edge between two S-blossoms.
                    // Pass it to the least-slack edge tracker.
                    lset_add_blossom_edge(bx, edge, slack);
                }

                if (ylabel != LABEL_S) {
                    // Found an to a T-vertex or unlabeled vertex "y".
                    // Pass it to the least-slack edge tracker.
                    // Tight edges must also be tracked in this way.
                    lset_add_vertex_edge(y, edge, slack);
                }
            }
        }

        // No further S vertices to scan, and no augmenting path found.
        return false;
    }

    /* **********  Delta steps:  ********** */

    /**
     * Calculate a delta step in the dual LPP problem.
     *
     * This function returns the minimum of the 4 types of delta values,
     * and the type of delta which obtain the minimum, and the edge or
     * blossom that produces the minimum delta, if applicable.
     *
     * This function takes time O(n).
     *
     * @pre There is at least one S-vertex.
     *
     * @return Tuple (delta_type, delta_value, delta_edge, delta_blossom)
     */
    DeltaStep substage_calc_dual_delta()
    {
        DeltaStep delta;
        delta.blossom = nullptr;

        // Compute delta1: minimum dual variable of any S-vertex.
        delta.kind = 1;
        delta.value = std::numeric_limits<WeightType>::max();
        for (VertexId x = 0; x < graph.num_vertex; ++x) {
            if (vertex_top_blossom[x]->label == LABEL_S) {
                delta.value = std::min(delta.value, vertex_dual[x]);
            }
        }

        // Compute delta2: minimum slack of any edge between an S-vertex and
        // an unlabeled vertex.
        const EdgeT* edge;
        WeightType slack;
        std::tie(edge, slack) = lset_get_best_vertex_edge();
        if ((edge != nullptr) && (slack <= delta.value)) {
            delta.kind = 2;
            delta.value = slack;
            delta.edge = edge->vt;
        }

        // Compute delta3: half minimum slack of any edge between two
        // top-level S-blossoms.
        std::tie(edge, slack) = lset_get_best_blossom_edge();
        if ((edge != nullptr) && (slack / 2 <= delta.value)) {
            delta.kind = 3;
            delta.value = slack / 2;
            delta.edge = edge->vt;
        }

        // Compute delta4: half minimum dual of a top-level T-blossom.
        for (NonTrivialBlossomT& blossom : nontrivial_blossom) {
            if ((! blossom.parent) && (blossom.label == LABEL_T)) {
                if (blossom.dual_var / 2 <= delta.value) {
                    delta.kind = 4;
                    delta.value = blossom.dual_var / 2;
                    delta.blossom = &blossom;
                }
            }
        }

        return delta;
    }

    /** Apply a delta step to the dual LPP variables. */
    void substage_apply_delta_step(WeightType delta)
    {
        // Apply delta to dual variables of all vertices.
        for (VertexId x = 0; x < graph.num_vertex; ++x) {
            BlossomLabel xlabel = vertex_top_blossom[x]->label;
            if (xlabel == LABEL_S) {
                // S-vertex: subtract delta from dual variable.
                vertex_dual[x] -= delta;
            } else if (xlabel == LABEL_T) {
                // T-vertex: add delta to dual variable.
                vertex_dual[x] += delta;
            }
        }

        // Apply delta to dual variables of top-level non-trivial blossoms.
        for (NonTrivialBlossomT& blossom : nontrivial_blossom) {
            if (blossom.parent == nullptr) {
                if (blossom.label == LABEL_S) {
                    // S-blossom: add 2*delta to dual variable.
                    blossom.dual_var += 2 * delta;
                } else if (blossom.label == LABEL_T) {
                    // T-blossom: subtract 2*delta from dual variable.
                    blossom.dual_var -= 2 * delta;
                }
            }
        }
    }

    /* **********  Main algorithm:  ********** */

    /**
     * Reset data which are only valid during a stage.
     *
     * Mark all blossoms as unlabeled, clear the queue,
     * reset least-slack edge tracking.
     */
    void reset_stage()
    {
        // Remove blossom labels.
        for (BlossomT& blossom : trivial_blossom) {
            blossom.label = LABEL_NONE;
        }
        for (BlossomT& blossom : nontrivial_blossom) {
            blossom.label = LABEL_NONE;
        }

        // Clear the S-vertex queue.
        queue.clear();

        // Reset least-slack edge tracking.
        lset_reset();
    }

    /**
     * Run one stage of the matching algorithm.
     *
     * The stage searches a maximum-weight augmenting path.
     * If this path is found, it is used to augment the matching,
     * thereby increasing the number of matched edges by 1.
     * If no such path is found, the matching must already be optimal.
     *
     * This function takes time O(n**2).
     *
     * @return True if the matching was successfully augmented;
     *         false if no further improvement is possible.
     */
    bool run_stage()
    {
        // Assign label S to all unmatched vertices and put them in the queue.
        for (VertexId x = 0; x < graph.num_vertex; ++x) {
            if (vertex_mate[x] == NO_VERTEX) {
                assign_label_s(x);
            }
        }

        // Stop if all vertices are matched.
        // No further improvement is possible in that case.
        // This avoids messy calculations of delta steps without any S-vertex.
        if (queue.empty()) {
            return false;
        }

        // Each pass through the following loop is a "substage".
        // The substage tries to find an augmenting path.
        // If an augmenting path is found, we augment the matching and end
        // the stage. Otherwise we update the dual LPP problem and enter the
        // next substage, or stop if no further improvement is possible.
        //
        // This loop runs through at most O(n) iterations per stage.
        bool augmented = false;
        while (true) {

            // Grow alternating trees.
            // End the stage if an augmenting path is found.
            augmented = substage_scan();
            if (augmented) {
                break;
            }

            // Calculate delta step in the dual LPP problem.
            DeltaStep delta = substage_calc_dual_delta();

            // Apply the delta step to the dual variables.
            substage_apply_delta_step(delta.value);

            if (delta.kind == 2) {
                // Use the edge from S-vertex to unlabeled vertex that got
                // unlocked through the delta update.
                VertexId x = delta.edge.first;
                VertexId y = delta.edge.second;
                if (vertex_top_blossom[x]->label != LABEL_S) {
                    std::swap(x, y);
                }
                assign_label_t(x, y);

            } else if (delta.kind == 3) {
                // Use the S-to-S edge that got unlocked by the delta update.
                // This may reveal an augmenting path.
                VertexId x = delta.edge.first;
                VertexId y = delta.edge.second;
                augmented = add_s_to_s_edge(x, y);
                if (augmented) {
                    break;
                }

            } else if (delta.kind == 4) {
                // Expand the T-blossom that reached dual value 0 through
                // the delta update.
                assert(delta.blossom);
                expand_t_blossom(delta.blossom);

            } else {
                // No further improvement possible. End the stage.
                assert(delta.kind == 1);
                break;
            }
        }

        // Remove all labels, clear queue.
        reset_stage();

        // Return True if the matching was augmented.
        return augmented;
    }

    /** Run the matching algorithm. */
    void run()
    {
        // Improve the solution until no further improvement is possible.
        //
        // Each successful pass through this loop increases the number
        // of matched edges by 1.
        //
        // This loop runs through at most (n/2 + 1) iterations.
        // Each iteration takes time O(n**2).
        while (run_stage()) ;
    }
};


/* **************************************************
 * **          struct MatchingVerifier             **
 * ************************************************** */

/** Helper class to verify that an optimal solution has been found. */
template <typename WeightType>
class MatchingVerifier
{
public:
    MatchingVerifier(const MatchingContext<WeightType>& ctx)
      : ctx(ctx),
        graph(ctx.graph),
        edge_duals(ctx.graph.edges.size())
    { }

    /**
     * Verify that the optimum solution has been found.
     *
     * This function takes time O(n**2).
     *
     * @return True if the solution is optimal; otherwise false.
     */
    bool verify()
    {
        return (verify_vertex_mate()
                && verify_vertex_duals()
                && verify_blossom_duals()
                && verify_blossoms_and_calc_edge_duals()
                && verify_edge_slack());
    }

private:
    using EdgeT = Edge<WeightType>;
    using BlossomT = Blossom<WeightType>;
    using NonTrivialBlossomT = NonTrivialBlossom<WeightType>;

    static bool checked_add(WeightType& result, WeightType a, WeightType b)
    {
        if (a > std::numeric_limits<WeightType>::max() - b) {
            return true;
        } else {
            result = a + b;
            return false;
        }
    }

    /** Convert edge pointer to its index in the vector "edges". */
    std::size_t edge_index(const EdgeT* edge)
    {
        return edge - graph.edges.data();
    }

    /** Check that the array "vertex_mate" is consistent. */
    bool verify_vertex_mate()
    {
        // Count matched vertices and check symmetry of "vertex_mate".
        VertexId num_matched_vertex = 0;
        for (VertexId x = 0; x < graph.num_vertex; ++x) {
            VertexId y = ctx.vertex_mate[x];
            if (y != NO_VERTEX) {
                ++num_matched_vertex;
                if (ctx.vertex_mate[y] != x) {
                    return false;
                }
            }
        }

        // Count matched edges.
        VertexId num_matched_edge = 0;
        for (const EdgeT& edge : graph.edges) {
            if (ctx.vertex_mate[edge.vt.first] == edge.vt.second) {
                ++num_matched_edge;
            }
        }

        // Check that all matched vertices correspond to matched edges.
        return (num_matched_vertex == 2 * num_matched_edge);
    }

    /**
     * Check that vertex dual variables are non-negative,
     * and all unmatched vertices have zero dual.
     */
    bool verify_vertex_duals()
    {
        for (VertexId x = 0; x < graph.num_vertex; ++x) {
            if (ctx.vertex_dual[x] < 0) {
                return false;
            }
            if ((ctx.vertex_mate[x] == NO_VERTEX) && (ctx.vertex_dual[x] != 0)) {
                return false;
            }
        }
        return true;
    }

    /** Check that blossom dual variables are non-negative. */
    bool verify_blossom_duals()
    {
        for (const NonTrivialBlossomT& blossom : ctx.nontrivial_blossom) {
            if (blossom.dual_var < 0) {
                return false;
            }
        }
        return true;
    }

    /**
     * Helper function for verifying the solution.
     *
     * Descend down the blossom tree to find edges that are contained
     * in blossoms.
     *
     * On the way down, keep track of the sum of dual variables of
     * containing blossoms. Add blossom duals to edges that are contained
     * inside blossoms.
     *
     * On the way up, keep track of the total number of matched edges
     * in subblossoms. Check that all blossoms with non-zero dual variables
     * are "full".
     *
     * @return True if successful;
     *         false if a blossom with non-zero dual is not full;
     *         false if blossom dual calculations cause numeric overflow.
     */
    bool check_blossom(const NonTrivialBlossomT* blossom)
    {
        // For each vertex "x",
        // "vertex_depth[x]" is the depth of the smallest blossom on
        // the current descent path that contains "x".
        std::vector<VertexId> vertex_depth(graph.num_vertex);

        // At each depth, keep track of the sum of blossom duals
        // along the current descent path.
        std::vector<WeightType> path_sum_dual = {0};

        // At each depth, keep track of the number of matched edges
        // along the current ascent path.
        std::vector<VertexId> path_num_matched = {0};

        // Use an explicit stack to avoid deep recursion.
        using SubBlossomList = std::list<typename NonTrivialBlossomT::SubBlossom>;
        std::stack<std::pair<const NonTrivialBlossomT*,
                             typename SubBlossomList::const_iterator>> stack;
        stack.emplace(blossom, blossom->subblossoms.begin());

        while (! stack.empty()) {
            VertexId depth = stack.size();
            auto& stack_elem = stack.top();

            blossom = stack_elem.first;
            auto subblossom_it = stack_elem.second;

            if (subblossom_it == blossom->subblossoms.begin()) {
                // We just entered this sub-blossom.
                // Update the depth of all vertices in this sub-blossom.
                for_vertices_in_blossom(blossom,
                    [&vertex_depth,depth](VertexId x) {
                        vertex_depth[x] = depth;
                    });

                // Calculate the sum of blossom duals at the new depth.
                path_sum_dual.push_back(path_sum_dual.back());
                if (checked_add(path_sum_dual.back(),
                                path_sum_dual.back(),
                                blossom->dual_var)) {
                    return false;
                }

                // Initialize the number of matched edges at the new depth.
                path_num_matched.push_back(0);

                if (blossom->subblossoms.size() < 3) {
                    return false;
                }
            }

            if (subblossom_it != blossom->subblossoms.end()) {

                // Update the sub-blossom pointer at the current depth.
                ++stack_elem.second;

                // Examine the current sub-blossom.
                BlossomT* sub = subblossom_it->blossom;
                NonTrivialBlossomT* ntb = sub->nontrivial();
                if (ntb) {
                    // Prepare to descend into this sub-blossom.
                    stack.emplace(std::make_pair(ntb, ntb->subblossoms.begin()));
                } else {
                    // Handle single vertex.
                    // For each incident edge, find the smallest blossom
                    // that contains it.
                    VertexId x = sub->base_vertex;
                    for (const EdgeT* edge : graph.adjacent_edges[x]) {
                        // Only consider edges pointing out from "x".
                        if (edge->vt.first == x) {
                            VertexId y = edge->vt.second;
                            VertexId edge_depth = vertex_depth[y];
                            if (edge_depth > 0) {
                                // Found the smallest blossom that contains this edge.
                                // Add the duals of the containing blossoms.
                                if (checked_add(edge_duals[edge_index(edge)],
                                                edge_duals[edge_index(edge)],
                                                path_sum_dual[edge_depth])) {
                                    return false;
                                }

                                // Update the number of matched edges in the blossom.
                                if (ctx.vertex_mate[x] == y) {
                                    path_num_matched[edge_depth] += 1;
                                }
                            }
                        }
                    }
                }

            } else {
                // We are leaving the current sub-blossom.

                // Count the number of vertices inside this blossom.
                VertexId blossom_num_vertex = 0;
                for_vertices_in_blossom(blossom,
                    [&blossom_num_vertex](VertexId) {
                        ++blossom_num_vertex;
                    });

                // Check that the blossom is "full".
                // A blossom is full if all except one of its vertices
                // are matched to another vertex within the blossom.
                VertexId blossom_num_matched = path_num_matched[depth];
                if (blossom_num_vertex != 2 * blossom_num_matched + 1) {
                    return false;
                }

                // Update the number of matched edges in the parent blossom.
                path_num_matched[depth - 1] += path_num_matched[depth];

                // Push vertices in this sub-blossom back to the parent.
                for_vertices_in_blossom(blossom,
                    [&vertex_depth,depth](VertexId x) {
                        vertex_depth[x] = depth - 1;
                    });

                // Trim the descending path.
                path_sum_dual.pop_back();
                path_num_matched.pop_back();

                // Remove the current blossom from the stack.
                stack.pop();
            }
        }

        return true;
    }

    /**
     * Check that all blossoms are full.
     * Also calculate the sum of dual variables for every edge.
     */
    bool verify_blossoms_and_calc_edge_duals()
    {
        // For each edge, calculate the sum of its vertex duals.
        for (const EdgeT& edge : graph.edges) {
            if (checked_add(edge_duals[edge_index(&edge)],
                            ctx.vertex_dual[edge.vt.first],
                            ctx.vertex_dual[edge.vt.second])) {
                return false;
            }
        }

        // Descend down each top-level blossom.
        // Check that blossoms are full.
        // Add blossom duals to the edges contained inside the blossoms.
        // This takes total time O(n**2).
        for (const NonTrivialBlossomT& blossom : ctx.nontrivial_blossom) {
            if (blossom.parent == nullptr) {
                if (!check_blossom(&blossom)) {
                    return false;
                }
            }
        }

        return true;
    }

    /**
     * Check that all edges have non-negative slack,
     * and check that all matched edges have zero slack.
     *
     * @pre Edge duals must be calculated before calling this function.
     */
    bool verify_edge_slack()
    {
        for (const EdgeT& edge : graph.edges) {
            WeightType duals = edge_duals[edge_index(&edge)];
            WeightType weight = ctx.weight_factor * edge.weight;

            if (weight > duals) {
                return false;
            }
            WeightType slack = duals - weight;

            if (ctx.vertex_mate[edge.vt.first] == edge.vt.second) {
                if (slack != 0) {
                    return false;
                }
            }
        }
        return true;
    }

private:
    /** Reference to the MatchingContext instance. */
    const MatchingContext<WeightType>& ctx;

    /** Reference to the input graph. */
    const Graph<WeightType>& graph;

    /**
     * For each edge, the sum of duals of its incident vertices
     * and duals of all blossoms that contain the edge.
     */
    std::vector<WeightType> edge_duals;
};


} // namespace impl


/* **************************************************
 * **        public functions                      **
 * ************************************************** */

/**
 * Compute a maximum-weighted matching in the general undirected weighted
 * graph given by "edges".
 *
 * The graph is specified as a list of edges, each edge specified as a tuple
 * of its two vertices and the edge weight.
 * There may be at most one edge between any pair of vertices.
 * No vertex may have an edge to itself.
 * The graph may be non-connected (i.e. contain multiple components).
 *
 * Vertices are indexed by consecutive, non-negative integers, such that
 * the first vertex has index 0 and the last vertex has index (n-1).
 * Edge weights may be integers or floating point numbers.
 *
 * Isolated vertices (not incident to any edge) are allowed, but not
 * recommended since such vertices consume time and memory but have
 * no effect on the maximum-weight matching.
 * Edges with negative weight are ignored.
 *
 * This function takes time O(n**3), where "n" is the number of vertices.
 * This function uses O(n + m) memory, where "m" is the number of edges.
 *
 * @tparam WeightType   Type used to represent edge weights.
 *                      For example "long" or "double".
 * @param  edges        Graph defined as a vector of weighted edges.
 *
 * @return Vector of pairs of matched vertex indices.
 *         This is a subset of the edges in the graph.
 *
 * @throw  std::invalid_argument  If the input graph is not valid.
 */
template <typename WeightType>
std::vector<VertexPair> maximum_weight_matching(
    const std::vector<Edge<WeightType>>& edges)
{
    // Check that the input meets all constraints.
    impl::check_input_graph(edges);

    // Run matching algorithm.
    impl::MatchingContext<WeightType> matching(edges);
    matching.run();

    // Verify that the solution is optimal (works only for integer weights).
    if (std::numeric_limits<WeightType>::is_integer) {
        assert(impl::MatchingVerifier<WeightType>(matching).verify());
    }

    // Extract the matched edges.
    std::vector<VertexPair> solution;
    for (const Edge<WeightType>& edge : matching.graph.edges) {
        if (matching.vertex_mate[edge.vt.first] == edge.vt.second) {
            solution.push_back(edge.vt);
        }
    }

    return solution;
}


/**
 * Adjust edge weights to compute a maximum cardinality matching.
 *
 * This function adjusts edge weights in the graph such that the
 * maximum-weight matching of the adjusted graph is a maximum-cardinality
 * matching, equal to a matching in the original graph that has maximum weight
 * out of all matchings with maximum cardinality.
 *
 * The graph is specified as a vector of edges.
 * Negative edge weights are allowed.
 *
 * This function increases all edge weights by an equal amount such that
 * the adjusted weights satisfy the following conditions:
 *   - All edge weights are positive;
 *   - The minimum edge weight is at least "n" times the difference between
 *     maximum and minimum edge weight.
 *
 * This function increases edge weights by an amount that is proportional
 * to the product of the unadjusted weight range and the number of vertices
 * in the graph. This may fail if it would cause adjusted edge weights
 * to exceed the supported numeric range. In case of floating point weights,
 * the weight adjustment may also cause increased rounding errors.
 *
 * This function takes time O(m), where "m" is the number of edges.
 *
 * @tparam WeightType   Type used to represent edge weights.
 * @param  edges        Graph defined as a vector of weighted edges.
 *
 * @return Vector of edges with adjusted weights.
 *         If no adjustments are necessary, this will be a copy of the
 *         input vector.
 *
 * @throw  std::invalid_argument  If the graph is invalid or edge weights
 *                                exceed the supported range.
 */
template <typename WeightType>
std::vector<Edge<WeightType>> adjust_weights_for_maximum_cardinality_matching(
    const std::vector<Edge<WeightType>>& edges_in)
{
    const WeightType min_safe_weight = std::numeric_limits<WeightType>::min() / 4;
    const WeightType max_safe_weight = std::numeric_limits<WeightType>::max() / 4;

    // Copy edges.
    std::vector<Edge<WeightType>> edges(edges_in);

    // Don't worry about empty graphs.
    if (edges.empty()) {
        return edges;
    }

    // Count number of vertices.
    // Determine minimum and maximum edge weight.
    VertexId num_vertex = 0;
    WeightType min_weight = edges.front().weight;
    WeightType max_weight = min_weight;

    const VertexId max_num_vertex = std::numeric_limits<VertexId>::max();
    for (const Edge<WeightType>& edge : edges) {
        VertexId m = std::max(edge.vt.first, edge.vt.second);
        if (m >= max_num_vertex) {
            throw std::invalid_argument("Vertex ID out of range");
        }
        num_vertex = std::max(num_vertex, m + 1);

        if (! std::numeric_limits<WeightType>::is_integer) {
            if (! std::isfinite(edge.weight)) {
                throw std::invalid_argument(
                    "Edge weights must be finite numbers");
            }
        }

        min_weight = std::min(min_weight, edge.weight);
        max_weight = std::max(max_weight, edge.weight);
    }

    // Calculate weight range and required weight adjustment.
    if ((min_weight < min_safe_weight) || (max_weight > max_safe_weight)) {
        throw std::invalid_argument(
            "Edge weight exceeds maximum supported value");
    }

    WeightType weight_range = max_weight - min_weight;

    if (weight_range > max_safe_weight / num_vertex) {
        throw std::invalid_argument(
            "Adjusted edge weight exceeds maximum supported value");
    }

    // Do nothing if the weights already ensure maximum-cardinality.
    if ((min_weight > 0) && (min_weight >= num_vertex * weight_range)) {
        return edges;
    }

    WeightType delta;
    if (weight_range > 0) {
        // Increase weights to make minimum edge weight large enough
        // to improve any non-maximum-cardinality matching.
        delta = num_vertex * weight_range - min_weight;
    } else {
        // All weights are the same. Increase weights to make them positive.
        delta = 1 - min_weight;
    }

    assert(delta >= 0);

    if (delta > max_safe_weight - max_weight) {
        throw std::invalid_argument(
            "Adjusted edge weight exceeds maximum supported value");
    }

    // Increase all edge weights by "delta".
    for (Edge<WeightType>& edge: edges) {
        edge.weight += delta;
    }

    return edges;
}


} // namespace mwmatching

#endif // MWMATCHING_H_