#include "edmonds.h"

using namespace ED;

namespace Edmonds {

/**
 * @return List of vertices of the x-r path, where r is the root of the
 * special blossom forest component x belongs to.
 *
 * @note This assumes that the values of μ, φ and ρ represent a special
 * blossom forest on the graph when this method is called.
 * **/
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_from_initial_matching(Graph & graph)
{
   graph.reset_forest();
   for(NodeId id = 0; id < graph.num_nodes(); ++id) {
      if (graph.is_outer(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
                  size_t distance_from_x = x_path.size() - 1;
                  size_t distance_from_y = y_path.size() - 1;
                  while (distance_from_x > 0 and distance_from_y > 0 and \
                     x_path[distance_from_x - 1] == y_path[distance_from_y - 1])
                  {
                     --distance_from_x;
                     --distance_from_y;
                  }
                  // found first vertex of x_path \cap y_path
                  while (graph.root_of_ear_component(x_path[distance_from_x]) != x_path[distance_from_x])
                  {
                     ++distance_from_x;
                     ++distance_from_y;
                  };
                  // found first vertex fixed by root_of_ear_component
                  NodeId blossom_root_id = x_path[distance_from_x];
                  for(size_t i = 1; i <= distance_from_x; i += 2)
                  {
                     if (graph.root_of_ear_component(graph.ear_or_root_neighbor(x_path[i])) != blossom_root_id)
                     {
                        graph.node(graph.ear_or_root_neighbor(x_path[i])).ear_or_root_neighbor = x_path[i];
                     }
                  }
                  for(size_t i = 1; i <= distance_from_y; i += 2)
                  {
                     if (graph.root_of_ear_component(graph.ear_or_root_neighbor(y_path[i])) != blossom_root_id)
                     {
                        graph.node(graph.ear_or_root_neighbor(y_path[i])).ear_or_root_neighbor = y_path[i];
                     }
                  }
                  if (graph.root_of_ear_component(x_path.front()) != blossom_root_id)
                  {
                     graph.node(x_path.front()).ear_or_root_neighbor = y_path.front();
                  }
                  if (graph.root_of_ear_component(y_path.front()) != blossom_root_id)
                  {
                     graph.node(x_path.front()).ear_or_root_neighbor = x_path.front();
                  }

                  // Update root indices
                  for(size_t i = 0; i <= distance_from_x; ++i)
                  {
                     graph.node(x_path[i]).root_of_ear_component = blossom_root_id;
                  }
                  for(size_t i = 0; i <= distance_from_y; ++i)
                  {
                     graph.node(y_path[i]).root_of_ear_component = blossom_root_id;
                  }
               }
            }
         }
      }
   }
   return graph;
};

void find_greedy_matching(Graph & graph)
{
   graph.reset_matching();
   for(NodeId node_id = 0; node_id < graph.num_nodes(); ++node_id)
   {
      if (graph.matched_neighbor(node_id) == node_id) {
         for(NodeId neighbor_id : graph.node(node_id).neighbors())
         {
            if(graph.matched_neighbor(neighbor_id) == neighbor_id)
            {
               graph.node(neighbor_id).matched_neighbor = node_id;
               graph.node(node_id).matched_neighbor = neighbor_id;
            }
         }
      }
   }
}

Graph maximum_matching(Graph & graph) {
   find_greedy_matching(graph);
   return maximum_matching_from_initial_matching(graph);
}

}