#include "edmonds.h"

using namespace ED;

namespace Edmonds {

std::vector<NodeId> path_to_forest_root(Graph const & graph, NodeId id)
{
   std::vector<NodeId> retval;
   retval.push_back(id);
   while (graph.matched_neighbor(id) != id)
   {
      id = graph.matched_neighbor(id);
      retval.push_back(id);

      // Note that it is guaranteed that this does not produce a loop:
      // We are traversing the path to a root of the forest,
      // but we know that each root is exposed by M, so after traversing
      // the matching edge, we cannot have reached a root.
      id = graph.ear_or_root_neighbor(id);
      retval.push_back(id);
   }
   return retval;
}


Graph maximum_matching(Graph & graph)
{
   graph.reset_forest();
   for(NodeId id = 0; id < graph.num_nodes(); ++id) {
      if (graph.is_out_of_forest(id))
      {
         for(NodeId neighbor_id : graph.node(id).neighbors())
         {
            if (graph.is_out_of_forest(neighbor_id))
            {
               // Grow Forest
               graph.node(neighbor_id).ear_or_root_neighbor = id;
            }
            else if (graph.is_outer(neighbor_id) and \
               graph.root_of_ear_component(id) != graph.root_of_ear_component(neighbor_id))
            {
               std::vector<NodeId> x_path = path_to_forest_root(graph, id);
               std::vector<NodeId> y_path = path_to_forest_root(graph, neighbor_id);

               if (x_path.back() != y_path.back())
               {
                  // Paths are disjoint -> augment
                  graph.node(x_path.front()).matched_neighbor = y_path.front();
                  graph.node(y_path.front()).matched_neighbor = x_path.front();

                  // TODO: put this into own method?
                  for(size_t i = 1; i < x_path.size(); i += 2)
                  {
                     graph.node(x_path[i]).matched_neighbor = x_path[i+1];
                     graph.node(x_path[i+1]).matched_neighbor = x_path[i];
                  }
                  for(size_t i = 1; i < y_path.size(); i += 2)
                  {
                     graph.node(y_path[i]).matched_neighbor = y_path[i+1];
                     graph.node(y_path[i+1]).matched_neighbor = y_path[i];
                  }
                  // Note that since this is tail-recursion, this will not generate
                  // new stack frames in OPT mode
                  return maximum_matching(graph);
               }
               else
               {
                  // Paths are not disjoint -> shrink blossom
               }
            }
         }
      }
   }
};
}