//
// MinWeightPerfectMatching.cpp
//
//	Adapted from C code by Jennifer Rich, Rice University
//	Adaptations by Jon Berry
//	Doesn't work for disconnected graphs (and no check is in place yet)
//
#ifndef __INTEGRAL_H
#define __INTEGRAL_H

/**************************************************************************/
/*                                                                        */
/* integral_match arguments:                                              */
/*    int nnodes:             input, the number of nodes in the graph     */
/*    int nedges:             input, the number of edges in the graph     */
/*    int end1[nedges]:       input, the first endpoint of the edges      */
/*    int end2[nedges]:       input, the second endpoint of the edges     */
/*    int weight[nedges]:     input, the cost of the edges                */
/*    int *value:             output, the value of the optimal matching   */
/*    int x[nedges]:          output, the optimal primal solution         */
/*    int dual[3*nnodes/2]:   output, the optimal dual solution.  The     */
/*                            duals of nodes [nnodes..3*nnodes/2) are the */
/*                            blossom's duals                             */
/*    int parent[3*nnodes/2]: output, the index of the smallest blossom   */
/*                            containing cur node/blossom, or -1 if      */
/*                            there is no containing blossom              */
/*                                                                        */
/*  return value: 0 if successful, 1 if the problem is infeasible,        */
/*                -1 if the algorithm failed (for example, out of memory) */
/*                                                                        */
/**************************************************************************/

int integral_match (int nnodes, int nedges, int *end1, int *end2,
		    int *weight, int *value, int *x, int *dual, int *parent);

#endif
#include <stdio.h>
#include <LINK/basic/Set.h>
#include <LINK/graph/Attribute.h>
#include <LINK/graph/Graph.h>
#include <LINK/graph/Edge.h>

#define INFINITY 1<<30
#define PLUS(currentmark) (currentmark)
#define MINUS(currentmark) ((currentmark)+1)
#define IS_UNLABELED(x,currentmark)  ((x)<(currentmark))
#define CLEAR_MARKS(currentmark) ((currentmark) +=2)

struct edgetype{
     int orig_e1;
     int orig_e2;
     int e1;
     int e2;
     int cost;
     int x;
     int rc;
};

struct nodetype{
     int in_cycle;
     int pi;
     int label;
     struct edge_ptr *adj;
     struct edgetype *matching_edge;
     struct edgetype *parent_edge;
     struct nodetype *parent;
     struct nodetype *child;
     struct nodetype *sib;
     struct nodetype *blossom_root;
     struct nodetype *blossom_parent;
     struct nodetype *penultimate;
};

struct edge_ptr{
     struct edgetype *cur;
     struct edge_ptr *next;
};

struct nodetype *node_free_list;

int grow_tree(struct nodetype *node_n, int *currentmark, 
                      struct edgetype *edges, struct nodetype *nodes);
int checkout_node(struct nodetype *node_n, int *currentmark, 
                  struct edgetype *edges, struct nodetype *nodes);
int add_to_tree(struct edgetype *e, int m, int p, int *currentmark, 
                         struct edgetype *edges, struct nodetype *nodes);
int
complement_path(int p, struct nodetype *end, struct nodetype *nodes);

int find_dual_change(struct nodetype *node_n, 
                                  int currentmark, struct edgetype *edges, 
                                  struct nodetype *nodes, 
                                  struct nodetype **minblossom);
int apply_dual_change(struct nodetype *node_n, int delta,
                                     int currentmark, struct edgetype *edges,
                                     struct nodetype *nodes);
struct nodetype *common_parent(struct nodetype *node_i, 
       struct nodetype *node_j, struct nodetype *nodes);
int 
form_blossom_cycle(struct edgetype *blossom_edge, int i, int j,
                   struct nodetype *blossom_root, struct nodetype *nodes);
int
shrink_tree(struct nodetype *_new);
int 
lift_edges(struct nodetype *_new, struct nodetype *nodes);
struct nodetype *shrink_blossom(struct edgetype *blossom_edge, int i, int j,
       struct nodetype *nodes);
int 
label_penultimate(struct nodetype *node_i, struct nodetype *label);
int 
fix_matching(struct nodetype *blossom, struct nodetype *exposed, 
             struct nodetype *nodes);
int
fix_tree(struct nodetype *blossom,struct nodetype *base,struct nodetype *root,
	 int currentmark);
int
lower_edges(struct nodetype *blossom, struct nodetype *nodes);
int 
expand_blossom(struct nodetype *node_n, struct nodetype *nodes, 
               int currentmark);
int 
fix_blossom_tree(int nnodes, struct nodetype *blossom, 
		 struct nodetype *node_x, struct nodetype *nodes);
int fixup(int nnodes, struct nodetype *nodes);

/*--- common_parent ---*/
/*--- returns the common tree parent of nodes i and j --- */


struct nodetype *common_parent(struct nodetype *node_i, 
       struct nodetype *node_j, struct nodetype *nodes)

{
     struct nodetype *iparent, *root;

     node_i->in_cycle = 1;
     iparent = node_i->parent;
     while ( iparent != NULL ) 
     {
          iparent->in_cycle = 1;
          iparent = iparent->parent;
     }
     if ( node_j->in_cycle ==  1 )
          root = node_j;
     else
     {
          node_j->in_cycle = 1;
          iparent = node_j->parent;
          while (iparent->in_cycle == 0)
          {
               iparent->in_cycle = 1;
               iparent = iparent->parent;
          }
          root = iparent;
     }
     iparent = root->parent;
     while ( iparent != NULL )
     {
          iparent->in_cycle = 0;
          iparent = iparent->parent;
     }

     return(root);
}


/*--- form_blossom_cycle ---*/

int 
form_blossom_cycle(struct edgetype *blossom_edge, int i, int j,
                   struct nodetype *blossom_root, struct nodetype *nodes)
{
     struct nodetype *prev, *pparent, *next;
     struct edgetype *prev_edge, *next_edge;

     prev = nodes + i;
     prev_edge = blossom_edge;
     pparent = nodes + j;
     while ( pparent != blossom_root ) 
     {
          next = pparent->parent;
          next_edge = pparent->parent_edge;
          pparent->parent = prev;
          pparent->parent_edge = prev_edge;
          prev = pparent;
          pparent = next;
          prev_edge = next_edge;
     }
     blossom_root->parent = prev;
     blossom_root->parent_edge = prev_edge;

     return(1);
}

/*--- shrink_tree ---*/
int
shrink_tree(struct nodetype *_new)

{
     struct nodetype *i, *c, *next_sib;

     _new->child = NULL;
     i=_new->blossom_root;
     _new->label = i->label;
     do {
          for (c=i->child; c; c=next_sib){
               next_sib = c->sib;
               if ( (c->blossom_parent != _new)  )
	       {
		 if (c->label != ((_new->label)+1))
		   next_sib = NULL;
		 else {
                    c->sib = _new->child;
                    _new->child = c;
                    c->parent = _new;
		 }
               }
          }
          i=i->parent;
     } while (i!= _new->blossom_root);     
     return(1);
}

/*--- lift_edges ---*/

int 
lift_edges(struct nodetype *_new, struct nodetype *nodes)

{
     struct nodetype *n;
     struct edge_ptr *pedge, *pedge_next;
     struct edgetype *e;
     int end1, end2;

     _new->adj = NULL;
     n=_new->blossom_root;
     do {
          for (pedge=n->adj, n->adj=NULL; pedge!=NULL; pedge=pedge_next){
               pedge_next = pedge->next;
               e=pedge->cur;
               end1 = e->e1;
               end2 = e->e2;
               if ( ((nodes[end1].blossom_parent)==_new) && 
                    ((nodes[end2].blossom_parent)==_new) )
	       {
                    pedge->next = n->adj;
                    n->adj = pedge;
               }
               else
	       {
                    if ( (nodes+end1) == n)
                         e->e1 = (_new-nodes);
                    else
                         e->e2 = (_new-nodes);
                    pedge->next = _new->adj;
                    _new->adj = pedge;
               }

          }
          n=n->parent;
     } while (n != (_new->blossom_root));
     return(1);
}

/*--- shrink_blossom ---*/

struct nodetype *shrink_blossom(struct edgetype *blossom_edge, int i, int j, 
				struct nodetype *nodes)

{
     struct nodetype *_new;
     struct nodetype *root, *save_parent, *p;
     struct edgetype *save_parent_edge;

     _new = node_free_list;
     node_free_list = node_free_list->parent;
     root = common_parent(nodes+i, nodes+j, nodes);
     save_parent = root->parent;
     save_parent_edge = root->parent_edge;
     form_blossom_cycle(blossom_edge, i, j, root, nodes);
     _new->blossom_root = root;
     _new->blossom_parent = NULL;
     p = root;
     do{
          p->blossom_parent = _new;
          p=p->parent;
     }while (p!=root);
     shrink_tree(_new);
     if (save_parent != NULL)
	 save_parent->child = _new;
     lift_edges(_new, nodes);
     _new->parent = save_parent;
     _new->parent_edge = save_parent_edge;
     _new->pi = 0;
     if (_new->blossom_root->matching_edge != NULL )
       _new->matching_edge = _new->blossom_root->matching_edge;
     return(_new);
}


/*--- label_penultimate ---*/

int 
label_penultimate(struct nodetype *node_i, struct nodetype *label)

{
     struct nodetype *j;

     if ( node_i->blossom_root == NULL )
          node_i->penultimate = label;
     else
       {
	 j = node_i->blossom_root;
	 do{
	   label_penultimate(j, label);
	   j=j->parent;
	 } while (j != node_i->blossom_root);
       }

     return(1);
}

/*--- fix_matching ---*/

int 
fix_matching(struct nodetype *blossom, struct nodetype *exposed, 
             struct nodetype *nodes)

{
     struct nodetype *root;

     root = blossom->blossom_root;
     if ( root != exposed ){
       if ( ((exposed->parent_edge)->x) == 1 ){
	       complement_path((exposed-nodes), root, nodes);
       }
       else {
               complement_path((root-nodes), exposed, nodes);
       }
     }
     blossom->blossom_root = exposed;
     return(1);
}

/*--- fix_tree ---*/

int
fix_tree(struct nodetype *blossom,struct nodetype *base,struct nodetype *root,
	 int currentmark)

{
     struct nodetype *p, *c, *next_sib, *next_c;
     struct edgetype *edge1, *edge2;

     p = blossom->parent;
     c=p->child;
     if ( c == blossom ){
          p->child = root;
     }
     else
       {
          next_sib=c->sib;
          while ( next_sib != blossom ) {
               c=next_sib;
	       next_sib=c->sib;
          }
          c->sib = root;
       }

     if ( root != base ){
       if ( root->parent_edge->x == 1)
       {
	 p=root;
	 p->label = MINUS(currentmark);
	 c = p->parent;
	 next_c = c->parent;
	 p->child = c;
	 edge1 = p->parent_edge;
	 do {
	   edge2=c->parent_edge;
	   c->label = PLUS(currentmark);
	   c->parent = p;
	   c->parent_edge = edge1;
	   c->child = next_c;
	   c->sib = NULL;
           p = c;
	   c = next_c;
	   next_c = next_c->parent;
	   edge1 = c->parent_edge;
	   c->label = MINUS(currentmark);
	   c->parent = p;
	   c->parent_edge = edge2;
	   c->child = next_c;
	   c->sib = NULL;
	   p = c;
	   c= next_c;
	   next_c = next_c->parent;
	 } while (p!=base);
       }
       else
       {
	 p=base;
	 p->label = MINUS(currentmark);
	 do {
	   c=p;
	   p=p->parent;
	   p->label = PLUS(currentmark);
	   p->child = c;
	   p->sib = NULL;
	   c=p;
	   p=p->parent;
	   p->label = MINUS(currentmark);
	   p->child = c;
	   p->sib = NULL;
	 } while (p!=root);	 
       }
     }
     else
       {
	 p=root;
	 p->label = MINUS(currentmark);
	 p=p->parent;
	 do{
	   c=p->parent;
	   p->parent_edge = NULL;
	   p->parent = NULL;
	   p=c;
	 }while (p!= root); 
       }
               
     root->parent = blossom->parent;
     root->parent_edge = blossom->parent_edge;
     root->sib = blossom->sib;
     base->child = blossom->child;
     blossom->child->parent = base;

     return(1);
}

/*--- lower_edges ---*/

int
lower_edges(struct nodetype *blossom, struct nodetype *nodes)

{
     struct edge_ptr *pedge, *pedge_next;
     struct edgetype *e;

     for (pedge = blossom->adj; pedge != NULL; pedge=pedge_next){
       pedge_next = pedge->next;
       e = pedge->cur;
       if ( e->e1 == (blossom-nodes) ){
	 e->e1 = (nodes[e->orig_e1].penultimate)-nodes;
	 pedge->next = nodes[e->e1].adj;
	 nodes[e->e1].adj = pedge;
       }
       else{
	 e->e2 = (nodes[e->orig_e2].penultimate)-nodes;
	 pedge->next = nodes[e->e2].adj;
	 nodes[e->e2].adj = pedge;
       }
     }
     return(1);
}

/*--- expand_blossom ---*/

int 
expand_blossom(struct nodetype *node_n, struct nodetype *nodes, 
               int currentmark)

{

  struct nodetype *i, *node_x, *node_y;
  struct edgetype *e;

  i=node_n->blossom_root;
  do{
    label_penultimate(i, i);
    i=i->parent;
  } while (i!= node_n->blossom_root);

  e = node_n->matching_edge;
  if ( e->e1 == (node_n - nodes) )
    node_x = nodes[e->orig_e1].penultimate;
  else
    node_x = nodes[e->orig_e2].penultimate;
  node_x->matching_edge = node_n->matching_edge;
  
  e = node_n->parent_edge;
  if ( e->e1 == (node_n - nodes) )
    node_y = nodes[e->orig_e1].penultimate;
  else
    node_y = nodes[e->orig_e2].penultimate;

  lower_edges(node_n, nodes);
  fix_matching(node_n, node_x, nodes);

  i=node_n->blossom_root;
  do{
    i->blossom_parent = NULL;
    i->in_cycle = 0;
    i->label = 0;
    i->child = NULL;
    i->sib = NULL;
    i=i->parent;
  } while (i!= node_n->blossom_root);

  fix_tree(node_n, node_x, node_y, currentmark);
  node_n->blossom_root = NULL;
  node_n->label =0;
  node_n->pi = 0;
  node_n->matching_edge = NULL;
  node_n->parent_edge = NULL;
  node_n->adj = NULL;
  node_n->child = NULL;
  node_n->sib = NULL;
  node_n->blossom_parent = NULL;
  node_n->penultimate = NULL;
  node_n->in_cycle = 0;
  node_n->parent = node_free_list;
  node_free_list = node_n;
  return(1);
}

/* --- fix_blossom_tree ---*/

int 
fix_blossom_tree(int nnodes, struct nodetype *blossom, 
		 struct nodetype *node_x, struct nodetype *nodes)

{
  struct nodetype *node, *i, *node_inside;
  struct edgetype *e;


  fix_matching(blossom, node_x, nodes);
  node = blossom->blossom_root;
  do {
    if ( node->blossom_root != NULL ){

      i=node->blossom_root;
      do{
	label_penultimate(i, i);
	i=i->parent;
      } while (i!= node->blossom_root);

      e = node->matching_edge;
      if ( e->e1 == (node - nodes) ){
	node_inside = nodes[e->orig_e1].penultimate;
	e->e1 = nodes[e->orig_e1].penultimate - nodes;
      }
      else {
	node_inside = nodes[e->orig_e2].penultimate;
	e->e2 = nodes[e->orig_e2].penultimate - nodes;
      }

      node_inside->matching_edge = node->matching_edge;
      fix_blossom_tree(nnodes, node, node_inside, nodes);
    }
    node = node->parent;
  }while (node != blossom->blossom_root);

  return(1);
}

/* --- fixup ---*/

int fixup(int nnodes, struct nodetype *nodes)

{
  int i;
  struct nodetype *j, *node_x;
  struct edgetype *e;


  for (i=nnodes; i<(3*nnodes/2); i++){
    if (nodes[i].blossom_root != NULL) {
	if (nodes[i].blossom_parent == NULL) {

		j=nodes[i].blossom_root;
		do{
		   label_penultimate(j, j);
		   j=j->parent;
		}while (j!= nodes[i].blossom_root);

	        e = nodes[i].matching_edge;
		if ( e->e1 == i ){
			node_x = nodes[e->orig_e1].penultimate;
			e->e1 = nodes[e->orig_e1].penultimate - nodes;
		}
      		else{
			node_x = nodes[e->orig_e2].penultimate;
			e->e2 = nodes[e->orig_e2].penultimate - nodes;
		}

		node_x->matching_edge = nodes[i].matching_edge;

      		fix_blossom_tree(nnodes, nodes+i, node_x, nodes);
	}
    }
  }
  return(1);
}




/* ----- find_dual_change -----*/

int
find_dual_change(struct nodetype *node_n, 
                             int currentmark, struct edgetype *edges, 
                             struct nodetype *nodes, 
                             struct nodetype **minblossom)

{
     int delta, n, m;
     int _newdelta;
     struct edge_ptr *pedge;
     struct edgetype *e;
     struct nodetype *c;
     struct nodetype *childblossom = (struct nodetype *) NULL;

     n = node_n - nodes;
     delta = INFINITY;

/* --- for nodes labeled +, bound the dual change by the minimum       ---*/
/* --- of the reduced cost of the edges connected to nodes not yet     ---*/
/* --- in the tree and the reduced cost divided by 2 of the edges      ---*/
/* --- connected to other nodes labeled with +                         ---*/
/* --- for blossoms labeled -, bound the dual by the dual at that node ---*/

     if (  node_n->label == PLUS(currentmark) )
     {
          for (pedge=node_n->adj; pedge!=NULL; pedge=pedge->next){
               e=pedge->cur;
               if ( e->e1 == n) 
                    m = e->e2;
               else 
                    m = e->e1;
               if (  (IS_UNLABELED(nodes[m].label, currentmark)) 
		     && ( e->rc < delta) )
	       {
                    delta = e->rc;
                    *minblossom = NULL;
               }
               else if ( (nodes[m].label == PLUS(currentmark)) 
			 && ( ((e->rc)/2) < delta) )
	       {
                    delta = ( (e->rc)/2);
                    *minblossom = NULL;
               }
          }
     }
     else if ( (node_n->label == MINUS(currentmark)) 
	       && (node_n->blossom_root != NULL) )
       {
	 if (node_n->pi <= delta)
	   {
	     delta = node_n->pi;
	     *minblossom = node_n;
	   }
       }

/* ---  for c in children(n) --- */
     for (c=node_n->child; c; c=c->sib){
          _newdelta = find_dual_change(c, currentmark, edges, 
                     nodes, &childblossom);
          if ( _newdelta < delta) {
               delta = _newdelta;
	       *minblossom = childblossom;
	   }
	  else{
	    if ( (_newdelta == delta) && (childblossom!= NULL) )
	      *minblossom = childblossom;
	  }
     }

     return(delta);
}

/* ----- apply_dual_change ----- */

int
apply_dual_change(struct nodetype *node_n, int delta,
                                int currentmark, struct edgetype *edges,
                                struct nodetype *nodes)

{
     struct edgetype *e;
     struct nodetype *c;
     struct edge_ptr *pedge;


/* --- for a node n labeled +, increase pi by delta and decrease --- */
/* --- the reduced cost by delta on edges coming out of n        --- */

     if (  node_n->label == PLUS(currentmark) ) {
          node_n->pi += delta;
          for (pedge=node_n->adj; pedge!=NULL; pedge=pedge->next){
               e=pedge->cur;
               e->rc -= delta;
          }
     }

/* --- for a node n labeled -, decrease pi by delta and increase --- */
/* --- the reduced cost by delta on edges coming out of n        --- */

     else if ( node_n->label == MINUS(currentmark) ) {
          node_n->pi -= delta;
          for (pedge=node_n->adj; pedge!=NULL; pedge=pedge->next){
               e=pedge->cur;
               e->rc += delta;
          }
     }
     else
          fprintf(stderr, "Error in apply dual change\n");


/* --- for c in children(n) --- */

     for (c=node_n->child; c; c=c->sib){
          apply_dual_change(c, delta, currentmark, edges, nodes);
     }
      
     return(1);
}




#include <stdio.h>
#include <stdlib.h>
//#include <unistd.h>
#include <string.h>
#include <sys/types.h>
//#include <sys/time.h>

int randnodes = 100;
int randedges = -1;
int def_maxweight = 1000000;
int def_minweight = 1000000;
char *filename = (char *) NULL;

static int init_graph (Graph *g, int *pnnodes, int *pnedges, int **pend1,
		       int **pend2, int **pweight);
static void parseargs (int ac, char **av);
static void usage (char *progname);
static void *mymalloc (int size);
static void double_weights (int ne, int *w);
static int check_matching (int nnodes, int nedges, int *end1, int *end2,
			   int *weight, int value, int *x, int *dual,
			   int *parent);

Set<Edge*> assign_matching_edges(Graph *g, int *x)
{
    Set<Edge*> result;
    const MSet<Edge*>& gedges = g->edges();
    Iterator<Edge*> get_edge(&gedges);
    Edge *e;
    while (get_edge(e)) {
	int erank;
	getAttribute((GraphObject*)e, "edge_rank", erank);
	if (x[erank]) {
		setAttribute((GraphObject*)e, "partition", 1);
		result.insert(e);
	} else
		setAttribute((GraphObject*)e, "partition", 0);
    }
    return result;
}

Set<Edge*> MinWeightPerfectMatching(Graph *g, char *graph_view)
{
    Set<Edge*> result;
    Attribute<int> edge_rank(g, "edge_rank", 0);
    int nnodes;
    int nedges;
    int *end1;
    int *end2;
    int *weight;
    int value;
    int *x;
    int *dual;
    int *parent;
    int retval;

    if (!g)
	return result;
    init_graph (g, &nnodes, &nedges, &end1, &end2, &weight);
    double_weights (nedges, weight);
    
    x = (int *) mymalloc (nedges * sizeof (int));
    dual = (int *) mymalloc (3*nnodes/2 * sizeof (int));
    parent = (int *) mymalloc (3*nnodes/2 * sizeof (int));

    retval = integral_match (nnodes, nedges, end1, end2, weight, &value,
			      x, dual, parent);

    printf ("integral_match return code %d\n", retval);
    printf ("Optimal value %d\n", value);

    if (retval == 0 &&
	!check_matching (nnodes, nedges, end1, end2, weight, value, x, dual,
			 parent)) {
	printf ("Optimal matching verified\n");
    } else {
	printf ("Matching failed\n");
    }
    result = assign_matching_edges(g, x);
    return result;
}

static int init_graph (Graph *g, int *pnnodes, int *pnedges, int **pend1,
		       int **pend2, int **pweight)
{
    int nnodes;
    int nedges;
    int *end1 = (int *) NULL;
    int *end2 = (int *) NULL;
    int *weight = (int *) NULL;
    int i;
    int using_stdin = 0;

    nnodes = g->order();
    nedges = g->size();
    end1 = (int *) mymalloc (nedges * sizeof (int));
    end2 = (int *) mymalloc (nedges * sizeof (int));
    weight = (int *) mymalloc (nedges * sizeof (int));
    Set<Edge*> gedges = g->edges();
    Set<Vertex*> gverts = g->vertices();
    Iterator<Edge*> get_edge(&gedges);
    Edge *e;
    i = 0; 
    while (get_edge(e)) {
	Vertex *v1 = e->sourceVertex();
	Vertex *v2 = e->sinkVertex();
	double dwgt;
	getAttribute((GraphObject*)e, "weight", dwgt);
	int wgt = (int) dwgt;
	end1[i] = gverts.rank(v1);
	end2[i] = gverts.rank(v2);
	weight[i] = wgt;
	setAttribute((GraphObject*)e, "edge_rank", i);
	i++;
    }
    *pnnodes = nnodes;
    *pnedges = nedges;
    *pend1 = end1;
    *pend2 = end2;
    *pweight = weight;
    
    return 0;
}

static void *mymalloc (int size)
{
    void *p;

    if (size <= 0) {
	fprintf (stderr, "Attempted to malloc %d bytes\n", size);
	exit (1);
    }
    p = malloc (size);
    if (p == (void *) NULL) {
	fprintf (stderr, "Failed to malloc %d bytes\n", size);
	exit (1);
    }
    return p;
}

static void double_weights (int ne, int *w)
{
    int i;

    for (i=0; i<ne; i++) {
	w[i] += w[i];
    }
}

static int set_depth (int i, int *depth, int *parent)
{
    int p = parent[i];
    int rval = 0;

    if (p == -1) {
	depth[i] = 0;
    } else {
	if (depth[p] == -1) {
	    depth[i] = -2;
	    rval = set_depth (p, depth, parent);
	    depth[i] = depth[p]+1;
	} else if (depth[p] == -2) {
	    printf ("ERROR: PARENT CYCLE: %d", i);
	    while (p != i) {
		printf (" %d", p);
		p = parent[p];
	    }
	    printf ("\n");
	    rval = 1;
	    depth[i] = 0;
	} else {
	    depth[i] = depth[p]+1;
	}
    }
    return rval;
}

static int check_matching (int nnodes, int nedges, int *end1, int *end2,
			   int *weight, int value, int *x, int *dual,
			   int *parent)
{
    int *deg, *depth;
    int i, j, k;
    int rc;
    int primalval;
    int dualval =0;
    int failed = 0;
    int depthfailed = 0;

    deg = (int *) mymalloc (nnodes * sizeof (int));
    depth = (int *) mymalloc (3*nnodes/2 * sizeof (int));

    for (i=0; i<3*nnodes/2; i++) depth[i] = -1;
    for (i=0; i<nnodes; i++) {
	depthfailed += set_depth (i, depth, parent);
    }

    for (i=0; i<nnodes; i++) deg[i] = 0;
    primalval = 0;
    for (i=0; i<nedges; i++) {
	if (x[i]) {
	    deg[end1[i]]++;
	    deg[end2[i]]++;
	    primalval += weight[i];
	}
	if (x[i] != 0 && x[i] != 1) {
	    printf ("Error, edge %d-%d has x value %d\n", end1[i],
		    end2[i], x[i]);
	    failed++;
	}
    }
    for (i=0; i<nnodes; i++) {
	if (deg[i] != 1) {
	    printf ("ERROR: PRIMAL INFEASIBLE: node %d has degree %d\n",
		    i, deg[i]);
	    if (deg[i]) {
		printf ("       matching edges:");
		for (j=0; j<nedges; j++) {
		    if (x[j] && (end1[j] == i || end2[j] == i)) {
			printf (" %d-%d", end1[j], end2[j]);
		    }
		}
		printf ("\n");
	    }
	    failed++;
	}
    }

    if (depthfailed) {
	printf ("CAN'T EVAULATE DUAL - PARENT HAD CYCLE\n");
    } else {
	dualval = 0;
	for (i=0; i<nnodes; i++) {
	    dualval += dual[i];
	}
	for (i=nnodes; i<3*nnodes/2; i++) {
	    if (depth[i] >= 0) {
		if (dual[i] < 0) {
		    printf ("ERROR: DUAL INFEASIBLE: blossom %d has negative dual %d\n", i, dual[i]);
		    failed++;
		}
		dualval += dual[i];
	    }
	}
	
	for (i=0; i<nedges; i++) {
	    rc = weight[i];
	    if (depth[end1[i]] < depth[end2[i]]) {
		j = end1[i];
		k = end2[i];
	    } else {
		j = end2[i];
		k = end1[i];
	    }
	    while (depth[k] > depth[j]) {
		rc -= dual[k];
		k = parent[k];
	    }
	    while (j != k) {
		rc -= dual[j];
		rc -= dual[k];
		j = parent[j];
		k = parent[k];
	    }
	    if (rc < 0) {
		printf ("ERROR: DUAL INFEASIBLE: edge %d-%d has reduced cost %d\n",
			end1[i], end2[i], rc);
		failed++;
	    } else if (rc > 0 && x[i]) {
		printf ("ERROR: edge %d-%d has x %d and reduced cost %d\n",
			end1[i], end2[i], x[i], rc);
	    }
	}
    }

    if (dualval != primalval) {
	printf ("ERROR: primal value %d != dual value %d\n",
		primalval, dualval);
        failed++;
    }

    if (primalval != value) {
	printf ("ERROR: primal value %d != stated value %d\n",
		primalval, value);
	failed++;
    }

    free ((void *) deg);
    free ((void *) depth);
    
    return failed;
}

static int random_graph (int nnodes, int nedges, int minweight,
			 int maxweight, int *end1, int *end2, int *weight)
{
    fprintf (stderr, "Random graphs not yet implemented\n");
    exit (1);
    return -1; /* makes acc happy */
}

#include <malloc.h>

/*-----------  weighted perfect matching procedure  ----------*/

int
integral_match (int nnodes, int nedges, int *end1, int *end2,
		     int *weight, int *value, int *x, int *dual, 
		     int*parent)

{
     struct edgetype *edges;
     struct nodetype *nodes=0;
     struct nodetype *minblossom;
     struct edge_ptr *adj_ptr=0, *pedge=0;
     int *degree=0;
     int delta=0, rval=0;
     int n, n2, i, j, count;
     int currentmark;


/*----- assign space for adjacency structures -----*/

     edges = (struct edgetype *) malloc(sizeof(struct edgetype) * nedges);
     if (edges == NULL){
          fprintf(stderr, "Error defining edges\n");
	  rval = -1;
	  goto CLEANUP;
     }

     nodes = (struct nodetype *) malloc(sizeof(struct nodetype) * 
              ((3*nnodes)/2));
     if (nodes == NULL){
          fprintf(stderr, "Error defining nodes\n");
	  rval = -1;
	  goto CLEANUP;
     }

     degree = (int *) malloc(sizeof(int) * nnodes);
     if (degree == NULL){
          fprintf(stderr, "Error defining degree\n");
	  rval = -1;
	  goto CLEANUP;
     }

     adj_ptr = (struct edge_ptr *) malloc(sizeof(struct edge_ptr)*2*nedges);
     if (adj_ptr == NULL){
          fprintf(stderr, "Error defining adj_ptr\n");
	  rval = -1;
	  goto CLEANUP;
     }

/*----- build adjacency structure -----*/

     node_free_list = &nodes[nnodes];
     for ( i=0; i< ((nnodes/2)); i++){
          if ( i < ((nnodes/2)-1) )
               node_free_list[i].parent = node_free_list+i+1;
          else 
               node_free_list[i].parent = NULL;
          node_free_list[i].in_cycle = 0;
          node_free_list[i].child = NULL;
          node_free_list[i].sib = NULL;
          node_free_list[i].matching_edge = NULL;
          node_free_list[i].parent_edge = NULL;
          node_free_list[i].label = 0;
          node_free_list[i].pi = INFINITY;
          node_free_list[i].blossom_root = NULL;
          node_free_list[i].blossom_parent = NULL;
          node_free_list[i].penultimate = NULL;
	  node_free_list[i].adj = NULL;
     }

     for (i=0; i<nnodes; i++){
          nodes[i].in_cycle = 0;
          nodes[i].parent = NULL;
          nodes[i].child = NULL;
          nodes[i].sib = NULL;
          nodes[i].matching_edge = NULL;
          nodes[i].parent_edge = NULL;
          degree[i] = 0;
          nodes[i].label = 0;
          nodes[i].pi = INFINITY;
          nodes[i].blossom_root = NULL;
          nodes[i].blossom_parent = NULL;
          nodes[i].penultimate = NULL;
     }

     for (i=0; i<nedges; i++){
          edges[i].e1 = end1[i];
          edges[i].orig_e1 = end1[i];
          edges[i].e2 = end2[i];
          edges[i].orig_e2 = end2[i];
          edges[i].cost = weight[i];
          edges[i].x = 0;
          degree[edges[i].e1]++;
          degree[edges[i].e2]++;
	  if ( (weight[i]%2) != 0 ){
	    printf("infeasible weight of edge %i is %i\n", i, weight[i]);
	    fflush(stderr);
	  }
     }


     count = 0;
     for (i=0; i<nnodes; i++){
          nodes[i].adj = adj_ptr + count;
          count += degree[i];
	  if ( degree[i] == 0 ) {
	    fprintf(stderr, "node %i has degree 0\n", i);
	    rval = -1;
	    goto CLEANUP;
	  }
          degree[i] = 0;
     }

     for (i=0; i<(2*nedges); i++){
       adj_ptr[i].next = NULL;
       adj_ptr[i].cur = NULL;
     }
     

     for (i=0; i<nedges; i++){
          j = edges[i].e1;
          nodes[j].adj[degree[j]].cur = &edges[i]; 
          if ( ((nodes[j].adj) + degree[j]+1) == (nodes[j+1].adj) )
               nodes[j].adj[degree[j]].next = NULL;
          else
               nodes[j].adj[degree[j]].next = &nodes[j].adj[degree[j]+1];
          degree[j]++;

          j = edges[i].e2;
          nodes[j].adj[degree[j]].cur = &edges[i]; 
          if ( ((nodes[j].adj) + degree[j]+1) == (nodes[j+1].adj) )
               nodes[j].adj[degree[j]].next = NULL;
          else
               nodes[j].adj[degree[j]].next = &nodes[j].adj[degree[j]+1];
          degree[j]++;
     }
     adj_ptr[(2*nedges)-1].next = NULL;


     for (i=0;i<nnodes;i++){
       for (pedge=nodes[i].adj; pedge; pedge = pedge->next){
	 if ((pedge->cur) == NULL){
	   printf("cur field empty for node %i\n", i);
	   fflush(stderr);
	   goto CLEANUP;
	 }
	 else {
	   if ( ( (pedge->cur->e1) != i) && ((pedge->cur->e2) != i)){
	   printf("cur field wrong for node %i\n", i);
	   fflush(stderr);
	   goto CLEANUP;
	   }
	 }
       }
     }

     free(degree);  
     degree = 0;

/* ----- initialize starting values for pi and rc ----- */

     for (i=0; i<nedges; i++){
          if (edges[i].cost < nodes[edges[i].e1].pi)
               nodes[edges[i].e1].pi = edges[i].cost;
          if (edges[i].cost < nodes[edges[i].e2].pi)
               nodes[edges[i].e2].pi = edges[i].cost;
     }

     for (i=0; i<nnodes; i++)
          nodes[i].pi = nodes[i].pi / 2;

     for (i=nnodes; i<(3*nnodes/2); i++)
       nodes[i].pi = 0;

     for (i=0; i<nedges; i++) 
          edges[i].rc = edges[i].cost - nodes[edges[i].e1].pi - nodes[edges[i].e2].pi;


/* ----- start of matching algorithm -----*/

    PLUS(currentmark) = 1;

    for (n2=0; n2<nnodes; n2++){ 
          if ( (nodes[n2].matching_edge == NULL))
          {
               n = n2;
               nodes[n].label = PLUS(currentmark);
               nodes[n].parent = NULL;
               nodes[n].parent_edge = NULL;
               nodes[n].sib = NULL;
	       minblossom = NULL;
               while ( !grow_tree(nodes+n, &currentmark, edges, nodes) )
               {
		    while (nodes[n].blossom_parent != NULL){
		      n=(nodes[n].blossom_parent) - nodes;
		    }
                    delta = find_dual_change(nodes+n, currentmark, 
                            edges, nodes, &minblossom);
                    if (delta == INFINITY){
                         fprintf(stderr,"No Perfect Matching exists!\n");
                          rval = -1;
			  goto CLEANUP;
                    }
		    apply_dual_change(nodes+n, delta, currentmark, edges, 
				      nodes);
		    if ( minblossom != NULL){
		      expand_blossom(minblossom, nodes, currentmark);
		      minblossom = NULL;
		    }
               }
          }
     }

    fixup(nnodes, nodes);
    rval = 0;
/* ----- assign value, x and dual -----*/

     *value = 0;
     for (i=0; i <nedges; i++) {
          x[i] = (edges[i].x);
          *value += (edges[i].cost )*(x[i]); 
     }

     for (i=0; i<(3*nnodes/2); i++){
       if (nodes[i].blossom_parent == NULL)
	    parent[i] = -1;
       else{
	    parent[i] = nodes[i].blossom_parent - nodes;
       }
       dual[i] = nodes[i].pi;
     }


/* ----- free memory ----- */
CLEANUP:
     if (degree) free(degree);  
     if (edges) free(edges);  
     if (nodes) free(nodes); 
     if (adj_ptr) free(adj_ptr); 
     return(rval);
}

/* This file includes grow_tree(), check_out_node(),  */
/* add_to_tree(), and complement_path()               */

/* -----grow_tree -----*/

int 
grow_tree(struct nodetype *node_n, int *currentmark, 
                 struct edgetype *edges, struct nodetype *nodes)

{
     struct nodetype *c;


     if (node_n->label == PLUS(*currentmark))
       if (checkout_node(node_n, currentmark, edges, nodes)){
               return(1);
       }

     if ( node_n->blossom_parent == NULL ){
       for (c=node_n->child; c; c=c->sib){
	 if (grow_tree(c, currentmark, edges, nodes)){
	       return(1);
	 }
	 if (node_n->blossom_parent != NULL){
	   return(0);
	 }
       }
     }
     return(0);
}

/*----- checkout_node -----*/
 
int
checkout_node(struct nodetype *node_n, int *currentmark, 
                         struct edgetype *edges, struct nodetype *nodes)

{
     struct edgetype *e;
     struct edge_ptr *pedge, *pedge_next;
     struct nodetype *_new;
     int i, k, m, p;

     for (pedge=node_n->adj; pedge!=NULL; pedge=pedge_next){
          if (node_n->blossom_parent != NULL)
	    return(0);
          pedge_next = pedge->next;
          e=pedge->cur;
          if ( e->rc == 0)
          {
               i = e->e1;
               k = e->e2;
               if ( (nodes + i) == node_n )
               {
                    m=k;
                    p=i;
               }
               else
               {
                    m=i;
                    p=k;
               }

	       if ( nodes[m].label == PLUS(*currentmark))
		 {
		   if ( (nodes[p].blossom_parent == NULL) || 
			(nodes[p].blossom_parent != nodes[m].blossom_parent) ){
		     _new = shrink_blossom(e, p, m, nodes);
		     return(checkout_node(_new, currentmark, edges, nodes));
		   }
		   else {
		     return(0);
		   }
		 }
               else if ( IS_UNLABELED(nodes[m].label,*currentmark) ){
		 if ( add_to_tree(e, m, p, currentmark, edges, nodes) ){
		   return(1);
		 }
	       }
          }
     }
     return(0);
}
 
/* ----- add_to_tree -----*/

int
add_to_tree(struct edgetype *e, int m, int p, int *currentmark, 
                    struct edgetype *edges, struct nodetype *nodes)

{
     /*   p is the node labeled +
          m is the unlabeled node  */

     int k;

/* ----- case 1:  edge goes from + to unmatched node ----- */

     if ( nodes[m].matching_edge == NULL )
     {
          e->x = 1;
          nodes[m].matching_edge = e;
          nodes[p].matching_edge = e;
          complement_path(p,NULL,nodes);
	  CLEAR_MARKS(*currentmark);
          return(1);
     }

/* ----- case 2:  edge goes from + to matched node ----- */

     else
     {
          if ( (nodes[m].matching_edge)->e1 ==  m )
               k =  (nodes[m].matching_edge)->e2;
          else 
               k =  (nodes[m].matching_edge)->e1;
          nodes[m].parent = nodes+p; 
          nodes[m].parent_edge = e;
          nodes[m].child = nodes + k;
          nodes[m].label = MINUS(*currentmark);
          nodes[m].sib = nodes[p].child;
          nodes[p].child = nodes + m;
          nodes[k].parent = nodes+m;
          nodes[k].sib = NULL;
          nodes[k].child = NULL;
          nodes[k].parent_edge = nodes[m].matching_edge;
          nodes[k].label = PLUS(*currentmark);
          return (checkout_node(nodes+k, currentmark, edges, nodes) );
     }
}

/* ----- complement_path ----- */

int
complement_path(int p, struct nodetype *end, struct nodetype *nodes)

{
     /* augment the path from p up to the end */
     /*  and clear all label information in tree */

     struct nodetype *parent_node;
     struct edgetype *augment_edge;


     if ( end == NULL )
     {               
       parent_node = nodes[p].parent;
       while ( parent_node != NULL ) {
          (parent_node->matching_edge)->x = 0;
          augment_edge =  parent_node->parent_edge;
          if (augment_edge == NULL)
          {
               fprintf(stderr, "Error augmenting path!\n");
               return(0);
          }
          else
          {
               augment_edge->x = 1;
               parent_node->matching_edge = augment_edge;
               parent_node = parent_node->parent;
               parent_node->matching_edge = augment_edge;
               parent_node = parent_node->parent;
          }
       }
     }
     else
     {
       parent_node = nodes+p;
       while ( parent_node != end ) {
	 if ( parent_node->parent_edge->x == 1){
               parent_node->parent_edge->x = 0;
	       if (parent_node->matching_edge == parent_node->parent_edge ){
		 printf("ERROR in complement path!!!!!!\n");
		 fflush(stderr);
	       }
	 }
          else
	    {
               augment_edge = parent_node->parent_edge;
	       augment_edge->x = 1;
               parent_node->matching_edge = augment_edge;
               parent_node->parent->matching_edge = augment_edge;
	    }
          parent_node = parent_node->parent;
       }
     }
     return(1);
}
