#include "edmonds.h"
#include <iostream>

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;
}

NodeId find_outer_vertex(Graph const & graph)
{
   for(NodeId id = 0; id < graph.num_nodes(); ++id)
   {
      if (not graph.node(id).scanned and graph.is_outer(id))
      {
         return id;
      }
   }
   return invalid_node_id;
}

void maximum_matching_from_initial_matching(Graph & graph)
{
   graph.reset_forest();
   NodeId id;
   while((id = find_outer_vertex(graph)) != invalid_node_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
               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;
               }
            }
         }
      }
      graph.node(id).scanned = true;
   }
};

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);
   maximum_matching_from_initial_matching(graph);

   ED::Graph matching = ED::Graph(graph.num_nodes());
   for (NodeId id = 0; id < graph.num_nodes(); ++id)
   {
      if (graph.matched_neighbor(id) > id)
      {
         matching.add_edge(id, graph.matched_neighbor(id));
      }
   }
   return matching;
}

}