void QPBO<REAL>::MergeParallelEdges()
{
	if (stage == 0) TransformToSecondStage(false);
	Node* i;
	Node* j;
	Arc* a;
	Arc* a_next;

	for (i=nodes[0]; i<node_last[0]; i++)
	{
		for (a=i->first; a; a=a->next)
		{
			j = a->head;
			if (!IsNode0(j)) j = GetMate1(j);
			j->parent = a;
		}
		for (a=i->first; a; a=a_next)
		{
			a_next = a->next;
			j = a->head;
			if (!IsNode0(j)) j = GetMate1(j);
			if (j->parent == a) continue;
			if (MergeParallelEdges(j->parent, a)==0) 
			{
				j->parent = a;
				a_next = a->next;
			}
		}
	}
}
	void QPBO<REAL>::Stitch()
{
	if (stage == 0) return;

	Node* i;
	Node* i_mate;
	Node* j;
	Arc* a;
	Arc* a_mate;

	for (a=arcs[0], a_mate=arcs[1]; a<arc_max[0]; a++, a_mate++)
	if (a->sister)
	{
		a->r_cap = a_mate->r_cap = a->r_cap + a_mate->r_cap;

		i = a->sister->head;
		j = a->head;

		if (i->region==0 || i->region != j->region) continue;
		if (IsNode0(i))
		{
			if (i->user_label != 0) continue;
		}
		else
		{
			if (GetMate1(i)->user_label != 1) continue;
		}
		if (IsNode0(j))
		{
			if (j->user_label != 1) continue;
		}
		else
		{
			if (GetMate1(j)->user_label != 0) continue;
		}

		a->r_cap = a_mate->r_cap = 0;
	}

	for (i=nodes[0], i_mate=nodes[1]; i<node_last[0]; i++, i_mate++)
	{
		i->tr_cap = i->tr_cap - i_mate->tr_cap;
		i_mate->tr_cap = -i->tr_cap;
	}

	ComputeWeakPersistencies();
}
	inline void QPBO<REAL>::add_to_changed_list(Node *i)
{
	if (keep_changed_list)
	{
		if (!IsNode0(i)) i = GetMate1(i);
		if (!i->is_in_changed_list)
		{
			Node** ptr = changed_list->New();
			*ptr = i;;
			i->is_in_changed_list = true;
		}
	}
}
	void QPBO<REAL>::test_consistency(Node* current_node)
{
	Node *i;
	Arc *a;
	int r;
	int num1 = 0, num2 = 0;

	// test whether all nodes i with i->next!=NULL are indeed in the queue
	for (i=nodes[0]; i<node_last[stage]; i++)
	{
		if (i==node_last[0]) i = nodes[1];
		if ((IsNode0(i) && i->is_removed) || (!IsNode0(i) && GetMate1(i)->is_removed))
		{
			code_assert(i->first == NULL);
			continue;
		}

		if (i->next || i==current_node) num1 ++;
	}
	for (r=0; r<3; r++)
	{
		i = (r == 2) ? current_node : queue_first[r];
		if (i)
		for ( ; ; i=i->next)
		{
			code_assert((IsNode0(i) && !i->is_removed) || (!IsNode0(i) && !GetMate1(i)->is_removed));
			num2 ++;
			if (i->next == i)
			{
				if (r<2) code_assert(i == queue_last[r]);
				else     code_assert(i == current_node);
				break;
			}
		}
	}
	code_assert(num1 == num2);

	for (i=nodes[0]; i<node_last[stage]; i++)
	{
		if (i==node_last[0]) i = nodes[1];
		if ((IsNode0(i) && i->is_removed) || (!IsNode0(i) && GetMate1(i)->is_removed)) continue;

		// test whether all edges in seach trees are non-saturated
		if (i->parent == NULL) {}
		else if (i->parent == QPBO_MAXFLOW_ORPHAN) {}
		else if (i->parent == QPBO_MAXFLOW_TERMINAL)
		{
			if (!i->is_sink) code_assert(i->tr_cap > 0);
			else             code_assert(i->tr_cap < 0);
		}
		else
		{
			if (!i->is_sink) code_assert (i->parent->sister->r_cap > 0);
			else             code_assert (i->parent->r_cap > 0);
		}
		// test whether passive nodes in search trees have neighbors in
		// a different tree through non-saturated edges
		if (i->parent && !i->next)
		{
			if (!i->is_sink)
			{
				code_assert(i->tr_cap >= 0);
				for (a=i->first; a; a=a->next)
				{
					if (a->r_cap > 0) code_assert(a->head->parent && !a->head->is_sink);
				}
			}
			else
			{
				code_assert(i->tr_cap <= 0);
				for (a=i->first; a; a=a->next)
				{
					if (a->sister->r_cap > 0) code_assert(a->head->parent && a->head->is_sink);
				}
			}
		}
		// test marking invariants
		if (i->parent && i->parent!=QPBO_MAXFLOW_ORPHAN && i->parent!=QPBO_MAXFLOW_TERMINAL)
		{
			code_assert(i->TS <= i->parent->head->TS);
			if (i->TS == i->parent->head->TS) code_assert(i->DIST > i->parent->head->DIST);
		}
	}
}
	void QPBO<REAL>::maxflow_reuse_trees_init()
{
	Node* i;
	Node* j;
	Node* queue = queue_first[1];
	Arc* a;
	nodeptr* np;

	queue_first[0] = queue_last[0] = NULL;
	queue_first[1] = queue_last[1] = NULL;
	orphan_first = orphan_last = NULL;

	TIME ++;

	while ((i=queue))
	{
		queue = i->next;
		if (queue == i) queue = NULL;
		if (IsNode0(i))
		{
			if (i->is_removed) continue;
		}
		else
		{
			if (GetMate1(i)->is_removed) continue;
		}
		i->next = NULL;
		i->is_marked = 0;
		set_active(i);

		if (i->tr_cap == 0)
		{
			if (i->parent) set_orphan_rear(i);
			continue;
		}

		if (i->tr_cap > 0)
		{
			if (!i->parent || i->is_sink)
			{
				i->is_sink = 0;
				for (a=i->first; a; a=a->next)
				{
					j = a->head;
					if (!j->is_marked)
					{
						if (j->parent == a->sister) set_orphan_rear(j);
						if (j->parent && j->is_sink && a->r_cap > 0) set_active(j);
					}
				}
				add_to_changed_list(i);
			}
		}
		else
		{
			if (!i->parent || !i->is_sink)
			{
				i->is_sink = 1;
				for (a=i->first; a; a=a->next)
				{
					j = a->head;
					if (!j->is_marked)
					{
						if (j->parent == a->sister) set_orphan_rear(j);
						if (j->parent && !j->is_sink && a->sister->r_cap > 0) set_active(j);
					}
				}
				add_to_changed_list(i);
			}
		}
		i->parent = QPBO_MAXFLOW_TERMINAL;
		i -> TS = TIME;
		i -> DIST = 1;
	}

	code_assert(stage == 1);
	//test_consistency();

	/* adoption */
	while ((np=orphan_first))
	{
		orphan_first = np -> next;
		i = np -> ptr;
		nodeptr_block -> Delete(np);
		if (!orphan_first) orphan_last = NULL;
		if (i->is_sink) process_sink_orphan(i);
		else            process_source_orphan(i);
	}
	/* adoption end */

	//test_consistency();
}
	void QPBO<REAL>::TransformToSecondStage(bool copy_trees)
{
	// add non-submodular edges
	Node* i[2];
	Node* j[2];
	Arc* a[2];

	memset(nodes[1], 0, node_num*sizeof(Node));
	node_last[1] = nodes[1] + node_num;

	if (!copy_trees)
	{
		for (i[0]=nodes[0], i[1]=nodes[1]; i[0]<node_last[0]; i[0]++, i[1]++)
		{
			i[1]->first = NULL;
			i[1]->tr_cap = -i[0]->tr_cap;
		}

		for (a[0]=arcs[0], a[1]=arcs[1]; a[0]<arc_max[0]; a[0]+=2, a[1]+=2)
		{
			if (!a[0]->sister) continue;

			code_assert(IsNode0(a[0]->sister->head));
			SET_SISTERS(a[1], a[1]+1);
			if (IsNode0(a[0]->head))
			{
				i[1] = GetMate0(a[0]->sister->head);
				j[1] = GetMate0(a[0]->head);

				SET_FROM(a[1],         j[1]);
				SET_FROM(a[1]->sister, i[1]);
				SET_TO(a[1],         i[1]);
				SET_TO(a[1]->sister, j[1]);
			}
			else
			{
				i[0] = a[0]->sister->head;
				i[1] = GetMate0(i[0]);
				j[1] = a[0]->head;
				j[0] = GetMate1(j[1]);

				SET_FROM(a[0],         i[0]);
				SET_FROM(a[0]->sister, j[1]);
				SET_FROM(a[1],         j[0]);
				SET_FROM(a[1]->sister, i[1]);
				SET_TO(a[1],         i[1]);
				SET_TO(a[1]->sister, j[0]);
			}
			a[1]->r_cap = a[0]->r_cap;
			a[1]->sister->r_cap = a[0]->sister->r_cap;
		}
	}
	else
	{
		for (i[0]=nodes[0], i[1]=nodes[1]; i[0]<node_last[0]; i[0]++, i[1]++)
		{
			i[1]->first = NULL;
			i[1]->tr_cap = -i[0]->tr_cap;
			i[1]->is_sink = i[0]->is_sink ^ 1;
			i[1]->DIST = i[0]->DIST;
			i[1]->TS = i[0]->TS;

			if (i[0]->parent == NULL || i[0]->parent == QPBO_MAXFLOW_TERMINAL) i[1]->parent = i[0]->parent;
			else i[1]->parent = GetMate0(i[0]->parent->sister);
		}

		for (a[0]=arcs[0], a[1]=arcs[1]; a[0]<arc_max[0]; a[0]+=2, a[1]+=2)
		{
			if (!a[0]->sister) continue;

			code_assert(IsNode0(a[0]->sister->head));
			SET_SISTERS(a[1], a[1]+1);
			if (IsNode0(a[0]->head))
			{
				i[1] = GetMate0(a[0]->sister->head);
				j[1] = GetMate0(a[0]->head);

				SET_FROM(a[1],         j[1]);
				SET_FROM(a[1]->sister, i[1]);
				SET_TO(a[1],         i[1]);
				SET_TO(a[1]->sister, j[1]);
			}
			else
			{
				i[0] = a[0]->sister->head;
				i[1] = GetMate0(i[0]);
				j[1] = a[0]->head;
				j[0] = GetMate1(j[1]);

				SET_FROM(a[0],         i[0]);
				SET_FROM(a[0]->sister, j[1]);
				SET_FROM(a[1],         j[0]);
				SET_FROM(a[1]->sister, i[1]);
				SET_TO(a[1],         i[1]);
				SET_TO(a[1]->sister, j[0]);

				mark_node(i[0]);
				mark_node(i[1]);
				mark_node(j[0]);
				mark_node(j[1]);
			}
			a[1]->r_cap = a[0]->r_cap;
			a[1]->sister->r_cap = a[0]->sister->r_cap;
		}
	}

	stage = 1;
}
	void QPBO<REAL>::AddPairwiseTerm(EdgeId e, NodeId _i, NodeId _j, REAL E00, REAL E01, REAL E10, REAL E11)
{
	user_assert(e >= 0 && arcs[0][2*e].sister);
	user_assert(arcs[0][2*e].head==&nodes[0][_i] || arcs[0][2*e].head==&nodes[1][_i] || arcs[0][2*e].head==&nodes[0][_j] || arcs[0][2*e].head==&nodes[1][_j]);
	user_assert(arcs[0][2*e+1].head==&nodes[0][_i] || arcs[0][2*e+1].head==&nodes[1][_i] || arcs[0][2*e+1].head==&nodes[0][_j] || arcs[0][2*e+1].head==&nodes[1][_j]);
	user_assert(_i != _j);

	REAL delta, ci, cj, cij, cji;

	if (stage == 0)
	{
		Arc* a = &arcs[0][2*e];
		Arc* a_rev = &arcs[0][2*e+1];
		code_assert(a->sister==a_rev && a->sister==a_rev);

		Node* i = a_rev->head;
		Node* j = a->head;
		code_assert(IsNode0(i));
		if (i != &nodes[0][_i]) { delta = E01; E01 = E10; E10 = delta; }
		if (IsNode0(j))
		{
			ComputeWeights(E00, E01, E10, E11, ci, cj, cij, cji);
			
			i->tr_cap += ci;
			j->tr_cap += cj;
			a->r_cap += cij;
			a_rev->r_cap += cji;

			if (a->r_cap < 0)
			{
				delta = a->r_cap;
				a->r_cap = 0;
				a_rev->r_cap += delta;
				i->tr_cap -= delta;
				j->tr_cap += delta;
			}
			if (a_rev->r_cap < 0)
			{
				delta = a_rev->r_cap;
				a_rev->r_cap = 0;
				a->r_cap += delta;
				j->tr_cap -= delta;
				i->tr_cap += delta;
			}

			if (a->r_cap < 0)
			{
				all_edges_submodular = false;
				REMOVE_FROM(a, i);
				REMOVE_FROM(a_rev, j);
				SET_TO(a, GetMate0(j));

				delta = a->r_cap;
				i->tr_cap -= delta;
				a->r_cap = -delta;
			}
		}
		else
		{
			j = GetMate1(j);
			ComputeWeights(E01, E00, E11, E10, ci, cj, cij, cji);
			
			i->tr_cap += ci;
			j->tr_cap -= cj;
			a->r_cap += cij;
			a_rev->r_cap += cji;

			if (a->r_cap < 0)
			{
				delta = a->r_cap;
				a->r_cap = 0;
				a_rev->r_cap += delta;
				i->tr_cap -= delta;
				j->tr_cap -= delta;
			}
			if (a_rev->r_cap < 0)
			{
				delta = a_rev->r_cap;
				a_rev->r_cap = 0;
				a->r_cap += delta;
				j->tr_cap += delta;
				i->tr_cap += delta;
			}

			if (a->r_cap < 0)
			{
				SET_FROM(a, i);
				SET_FROM(a_rev, j);
				SET_TO(a, j);

				delta = a->r_cap;
				i->tr_cap -= delta;
				a->r_cap = -delta;
			}
		}
	}
	else
	{
		Arc* a[2] = { &arcs[0][2*e], &arcs[1][2*e] };
		Arc* a_rev[2] = { &arcs[0][2*e+1], &arcs[1][2*e+1] };
		code_assert(a[0]->sister==a_rev[0] && a[1]->sister==a_rev[1] && a[0]==a_rev[0]->sister && a[1]==a_rev[1]->sister);

		Node* i[2] = { a_rev[0]->head, a[1]->head };
		Node* j[2] = { a[0]->head, a_rev[1]->head };
		int k = IsNode0(i[0]) ? 0 : 1;
		if (i[k] != &nodes[0][_i]) { delta = E01; E01 = E10; E10 = delta; }
		if (IsNode0(j[k]))
		{ 
			ComputeWeights(E00, E01, E10, E11, ci, cj, cij, cji);
		}
		else
		{ 
			ComputeWeights(E01, E00, E11, E10, ci, cj, cij, cji);
		};

		// make sure that a[0]->r_cap == a[1]->r_cap and a_rev[0]->r_cap == a_rev[1]->r_cap by pushing flow
		delta = a[1]->r_cap - a[0]->r_cap;
		//a[1]->r_cap -= delta;   // don't do the subtraction - later we'll set explicitly a[1]->r_cap = a[0]->r_cap
		//a[1]->sister->r_cap += delta;
		a_rev[1]->head->tr_cap -= delta;
		a[1]->head->tr_cap     += delta;

		i[0]->tr_cap += ci; i[1]->tr_cap -= ci;
		j[0]->tr_cap += cj; j[1]->tr_cap -= cj;
		a[0]->r_cap += cij;
		a_rev[0]->r_cap += cji;

		if (a[0]->r_cap < 0)
		{
			delta = a[0]->r_cap;
			a[0]->r_cap = 0;
			a_rev[0]->r_cap += delta;
			i[0]->tr_cap -= delta; i[1]->tr_cap += delta;
			j[0]->tr_cap += delta; j[1]->tr_cap -= delta;
		}
		if (a_rev[0]->r_cap < 0)
		{
			delta = a_rev[0]->r_cap;
			a_rev[0]->r_cap = 0;
			a[0]->r_cap += delta;
			j[0]->tr_cap -= delta; j[1]->tr_cap += delta;
			i[0]->tr_cap += delta; i[1]->tr_cap -= delta;
		}

		if (a[0]->r_cap < 0)
		{
			// need to swap submodular <-> supermodular
			SET_TO(a[0], j[1]);
			SET_TO(a_rev[1], j[0]);
			REMOVE_FROM(a_rev[0], j[0]);
			SET_FROM(a_rev[0], j[1]);
			REMOVE_FROM(a[1], j[1]);
			SET_FROM(a[1], j[0]);

			delta = a[0]->r_cap;
			i[0]->tr_cap -= delta; i[1]->tr_cap += delta;
			a[0]->r_cap = -delta;
		}

		a[1]->r_cap = a[0]->r_cap;
		a_rev[1]->r_cap = a_rev[0]->r_cap;
	}

	zero_energy += E00;
}
	bool QPBO<REAL>::Save(char* filename, int format)
{
	int e;
	int edge_num = 0;
	for (e=GetNextEdgeId(-1); e>=0; e=GetNextEdgeId(e)) edge_num ++;

	if (format == 0)
	{
		FILE* fp;
		REAL E0, E1, E00, E01, E10, E11;
		int i, j;
		char* type_name;
		char* type_format;
		char FORMAT_LINE[64];
		int factor = (stage == 0) ? 2 : 1;

		get_type_information(type_name, type_format);

		fp = fopen(filename, "w");
		if (!fp) return false;

		fprintf(fp, "nodes=%d\n", GetNodeNum());
		fprintf(fp, "edges=%d\n", edge_num);
		fprintf(fp, "labels=2\n");
		fprintf(fp, "type=%s\n", type_name);
		fprintf(fp, "\n");

		sprintf(FORMAT_LINE, "n %%d\t%%%s %%%s\n", type_format, type_format);
		for (i=0; i<GetNodeNum(); i++)
		{
			GetTwiceUnaryTerm(i, E0, E1);
			REAL delta = (E0 < E1) ? E0 : E1;
			fprintf(fp, FORMAT_LINE, i, (E0-delta)/factor, (E1-delta)/factor);
		}
		sprintf(FORMAT_LINE, "e %%d %%d\t%%%s %%%s %%%s %%%s\n", type_format, type_format, type_format, type_format);
		for (e=GetNextEdgeId(-1); e>=0; e=GetNextEdgeId(e))
		{
			GetTwicePairwiseTerm(e, i, j, E00, E01, E10, E11);
			fprintf(fp, FORMAT_LINE, i, j, E00/factor, E01/factor, E10/factor, E11/factor);
		}
		fclose(fp);
		return true;
	}
	
	if (format == 1)
	{
		FILE* fp;
		REAL E0, E1, E00, E01, E10, E11;
		int i, j;
		Arc* a;
		char* type_name;
		char* type_format;

		if (stage == 0) Solve();

		get_type_information(type_name, type_format);
		if (type_format[0] != 'd') return false;

		fp = fopen(filename, "w");
		if (!fp) return false;

		fprintf(fp, "p %d %d\n", GetNodeNum(), GetNodeNum() + edge_num);

		for (i=0; i<GetNodeNum(); i++)
		{
			GetTwiceUnaryTerm(i, E0, E1);
			REAL delta = E1 - E0;
			for (a=nodes[0][i].first; a; a=a->next)
			{
				if (IsNode0(a->head)) delta += a->sister->r_cap + GetMate(a)->sister->r_cap;
				else                  delta -= a->r_cap + GetMate(a)->r_cap;
			}
			fprintf(fp, "1 %d %d\n", i+1, delta);
		}
		for (e=GetNextEdgeId(-1); e>=0; e=GetNextEdgeId(e))
		{
			GetTwicePairwiseTerm(e, i, j, E00, E01, E10, E11);
			fprintf(fp, "2 %d %d %d\n", i+1, j+1, E00 + E11 - E01 - E10);
		}
		fclose(fp);
		return true;
	}
	return false;
}