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

using namespace ED;

namespace Edmonds {

void check_integrity(Graph const & graph)
{
   for(NodeId id = 0; id < graph.num_nodes(); ++id)
   {
      NodeId matched = graph.matched_neighbor(id);
      if(matched != id)
      {
         assert(graph.matched_neighbor(matched) == id);
      }

      if (not graph.is_outer(id))
      {
         assert(graph.rho(id) == id);
      }
   }
}

/**
 * @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.phi(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())
      {
         check_integrity(graph);
         std::cout << "Check passed" << std::endl;
         if (graph.is_out_of_forest(neighbor_id))
         {
            std::cout << "Grow forest" << std::endl;
            // Grow Forest
            graph.node(neighbor_id).phi = id;
         }
         else if (graph.is_outer(neighbor_id) and graph.rho(id) != graph.rho(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())
            {
               std::cout << "Augment" << std::endl;
               // 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
               graph.reset_forest();
               break;
            }
            else
            {
               std::cout << "Contract blossom" << std::endl;
               // 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.rho(x_path[distance_from_x]) != x_path[distance_from_x])
               {
                  ++distance_from_x;
                  ++distance_from_y;
               };
               // found first vertex fixed by rho
               NodeId blossom_root_id = x_path[distance_from_x];
               for(size_t i = 1; i <= distance_from_x; i += 2)
               {
                  if (graph.rho(graph.phi(x_path[i])) != blossom_root_id)
                  {
                     graph.node(graph.phi(x_path[i])).phi = x_path[i];
                  }
               }
               for(size_t i = 1; i <= distance_from_y; i += 2)
               {
                  if (graph.rho(graph.phi(y_path[i])) != blossom_root_id)
                  {
                     graph.node(graph.phi(y_path[i])).phi = y_path[i];
                  }
               }
               if (graph.rho(x_path.front()) != blossom_root_id)
               {
                  graph.node(x_path.front()).phi = y_path.front();
               }
               if (graph.rho(y_path.front()) != blossom_root_id)
               {
                  graph.node(x_path.front()).phi = x_path.front();
               }

               // Update root indices
               for(size_t i = 0; i <= distance_from_x; ++i)
               {
                  graph.node(x_path[i]).rho = blossom_root_id;
               }
               for(size_t i = 0; i <= distance_from_y; ++i)
               {
                  graph.node(y_path[i]).rho = 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);
   graph.reset_matching();
   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;
}

}