// -*- C++ -*-
/*
Kell meg a dual oldal
esetleg az elejen moho matching kereses

  //baj van pos-sal: nem tudok vegigmaszni a halmazok elemein. De
  //pos-ra nem lenne szukseg ha unionfindban lenne egy element ami
  //true ha benne van false ha nincs

MISI strukijahoz hasznalok:

void addElement(b,a): b meg nem volt egy osztalyban sem, ezzel hozzaadjuk a osztalyahoz
void insert(a): a meg nem volt egy osztalyban sem, ezzel egy uj egyelemu osztalyt alkot
bool element(a): true ha benne van false ha nincs
void erase(a)

ez a 4 a vegso szetrobbantashoz es egyideju elemfelsorolashoz kell, mas megoldas is jo ehelyett:

bool empty(int i): ures-e az i nevu osztaly
int find(a)
void pop(int i): egy fix elso elemet kitorol az i osztalybol
T top(int i): visszad egy fix elso elemet 

unionfindba kene:
bool element(a): true ha benne van false ha nincs  (vagy 'bool empty')
void split(a) (vagy clear)
T rep(a): visszaadja a halmazanak reprezentanselemet, lehet operator()-val is
void makeRep(a): a-t a sajat halmazanak reprezentansava teszi

Szoljatok ha nem lehet valamelyiket hatekonyan
*/

#ifndef HUGO_EDMONDS_H
#define HUGO_EDMONDS_H

#include <queue>

#include <invalid.h>
#include <unionfind.h>

namespace hugo {

  template <typename Graph>
  class Edmonds {
    
    typedef typename Graph::Node Node;
    typedef typename Graph::Edge Edge;
    typedef typename Graph::EdgeIt EdgeIt;
    typedef typename Graph::NodeIt NodeIt;
    typedef typename Graph::OutEdgeIt OutEdgeIt;

    typedef UnionFindEnum<Node, Graph::NodeMap > ufe;
            
    const Graph& G;

  public:
    Edmonds(Graph& _G) : G(_G) {}
    
    
    int run(std::vector<Edge> matching) {

      enum pos_enum{
	D=0,
	A=1,
	C=2
      };
      Graph::template NodeMap<pos_enum> pos(G,C);

      int p=0;   //needed for path

      typename Graph::NodeMap<Node> mate(G,INVALID);
      for ( int i=0; i!=matching.size(); ++i ) {
	Node u=G.aNode(matching[i]);
	Node v=G.bNode(matching[i]);
	mate.set(u,v);
	mate.set(v,u);
      }
      matching.clear();

      typename Graph::NodeMap<Node> ear(G,INVALID);   
      // a basepontok es a C-beliek earje szemet

      ufe::MapType blossom_base(G);
      ufe blossom(blossom_base);
      ufe::MapType tree_base(G);
      ufe tree(tree_base);

      NodeIt v;
      for(G.first(v); G.valid(v), G.next(v) ) {
	
	if ( pos[v]==C && mate[v]=INVALID ) {
	  
	  blossom.insert(v);
	  tree.insert(v); 
	  pos.set(v,D);

	  std::queue<Node> Q;   //a vizsgalando csucsok sora
	  Q.push(v);  

	  while ( !Q.empty() ) {
	    Node x=Q.top();
	    Q.pop();
	    
	    OutEdgeIt e;
	    for( G.first(e); G.valid(e), G.next(e) ) {
	      Node y=G.bNode(e);

	      if ( pos[y]==D ) { 

		if ( tree.find(x) != tree.find(y) ) {  //augment 
		  augment(x, mate, ear, blossom, tree); 
		  augment(y, mate, ear, blossom, tree); 
		  mate.set(x)=y;
		  mate.set(y)=x;
		  goto increased;
		} else 
		  if ( blossom.find(x) != blossom.find(y) ) {

		    typename Graph::NodeMap<int> path(G,0);

		    ++p;
		    Node b=blossom.find(x);
		    path.set(b,p);
		    while ( b!=INVALID ) {
		      b=blossom.find(ear[mate[b]]);
		      path.set(b,p);
		    }          //going till the root
	
		    Node w=y;
		    Node v=blossom.find(w);
		    while ( path[v]!=p ) {		      
		      Q.push(mate[v]);
		      w=shrink_step(w,v,mate,ear,tree,blossom);
		      v=blossom.find(w);
		    }
		    //now v is the base of the first common ancestor

		    w=x;
		    Node z=blossom.find(w);
		    while ( z!=v ) {
		      Q.push(mate[z]);
		      w=shrink_step(w,z,mate,ear,tree,blossom);
		      z=blossom.find(w);
		    }

		    ear.set(x,y);
		    ear.set(y,x);

		    blossom.makeRep(v);
		  }
	      } else if ( pos[y] == C ) 
		
		if ( mate[y]!=INVALID ) {       //grow
		  ear.set(y,x);
		  Node w=mate(y);
		  blossom.insert(w);
		  tree.insert(w);
		  tree.join(y,x);  
		  tree.join(w,x);  
		  Q.push(w);
		} else {
		  augment(x, mate, ear, blossom, tree); 
		  mate.set(x)=y;
		  mate.set(y)=x;
		  goto increased;
		}
	    }
	  }
	increased:  //Misi, warningol, hogy nincs utasitas!
	}
      }

      int s=0;
      NodeIt v;
      for(G.first(v); G.valid(v), G.next(v) ) 
	if ( mate[v]!=INVALID ) ++s;
	
      return (int)(s/2);

      //egyelore nem ad semmit vissza
      
    }
  

    void augment(const Node x, typename Graph::NodeMap<Node>& mate, 
		 typename Graph::NodeMap<Node>& ear, 
		 ufe& blossom, ufe& tree) { 
      Node v=mate[x];
      while ( v!=INVALID ) {
	Node u=ear[v];
	mate.set(v,u);
	Node tmp=v;
	v=mate[u];
	mate.set(u,tmp);
      }
      //FIXME, blossom szetszed
      tree.eraseClass(x);
    }


    Node shrink_step(const Node z, const Node v, typename Graph::NodeMap<Node>& mate, 
		     typename Graph::NodeMap<Node>& ear, 
		     ufe& blossom, ufe& tree) {  
      if ( z!=v ) {
	Node t=z;
	Node u;
	while ( t!=v ) {
	  u=mate[t];
	  t=ear[u];
	}
	ear.set(v,u);
      }

      Node u=mate[v];
      Node w=ear[u];
      tree.erase(u);
      tree.erase(v);
      blossom.insert(u);
      blossom.join(u,w);
      blossom.join(v,w);
      ear.set(w,u);      

      return w;
    }


  };

} //namespace hugo

#endif //EDMONDS_H