void QPBO<REAL>::Solve()
{
	Node* i;

	maxflow();

	if (stage == 0)
	{
		if (all_edges_submodular)
		{
			for (i=nodes[0]; i<node_last[0]; i++)
			{
				i->label = what_segment(i);
			}
			return;
		}

		TransformToSecondStage(true);
		maxflow(true);
	}

	for (i=nodes[0]; i<node_last[0]; i++)
	{
		i->label = what_segment(i);
		if (i->label == what_segment(GetMate0(i))) i->label = -1;
	}
}
	void QPBO<REAL>::TestRelaxedSymmetry()
{
	Node* i;
	Arc* a;
	REAL c1, c2;

	if (stage == 0) return;

	for (i=nodes[0]; i<node_last[0]; i++)
	{
		if (i->is_removed) continue;
		c1 = i->tr_cap;
		for (a=i->first; a; a=a->next) c1 += a->sister->r_cap;
		c2 = -GetMate0(i)->tr_cap;
		for (a=GetMate0(i)->first; a; a=a->next) c2 += a->r_cap;
		if (c1 != c2)
		{
			code_assert(0);
			exit(1);
		}
	}
}
	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;
}
	typename QPBO<REAL>::EdgeId QPBO<REAL>::AddPairwiseTerm(NodeId _i, NodeId _j, REAL E00, REAL E01, REAL E10, REAL E11)
{
	//printf("%d,%d",_i,node_num);
	user_assert(_i >= 0 && _i < node_num);
	user_assert(_j >= 0 && _j < node_num);
	user_assert(_i != _j);

	REAL ci, cj, cij, cji;

	if (!first_free) 
	{
		reallocate_arcs(2*(GetMaxEdgeNum() + GetMaxEdgeNum()/2));
	}

	EdgeId e = (int)(first_free - arcs[IsArc0(first_free) ? 0 : 1])/2;
	first_free = first_free->next;

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

		Node* i = nodes[0] + _i;
		Node* j = nodes[0] + _j;

		if (E01 + E10 >= E00 + E11)
		{
			ComputeWeights(E00, E01, E10, E11, ci, cj, cij, cji);

			SET_TO(a, j);
			SET_FROM(a,     i);
			SET_FROM(a_rev, j);

			j->tr_cap += cj;
		}
		else
		{
			all_edges_submodular = false;
			ComputeWeights(E01, E00, E11, E10, ci, cj, cij, cji);

			SET_TO(a, GetMate0(j));
			a->next = NULL;
			a_rev->next = NULL;

			j->tr_cap -= cj;
		}

		SET_SISTERS(a, a_rev);
		SET_TO(a_rev, i);

		i->tr_cap += ci;
		a->r_cap = cij;
		a_rev->r_cap = cji;
	}
	else
	{
		Arc *a[2], *a_rev[2];
		a[0]     = &arcs[0][2*e];
		a_rev[0] = &arcs[0][2*e+1];
		a[1]     = &arcs[1][2*e];
		a_rev[1] = &arcs[1][2*e+1];

		Node* i[2] = { nodes[0] + _i, nodes[1] + _i };
		Node* j[2];

		if (E01 + E10 >= E00 + E11)
		{
			j[0] = nodes[0] + _j; j[1] = nodes[1] + _j;
			ComputeWeights(E00, E01, E10, E11, ci, cj, cij, cji);
		}
		else
		{
			j[1] = nodes[0] + _j; j[0] = nodes[1] + _j;
			ComputeWeights(E01, E00, E11, E10, ci, cj, cij, cji);
		}

		SET_SISTERS(a[0], a_rev[0]);
		SET_SISTERS(a[1], a_rev[1]);

		SET_TO(a[0],     j[0]);
		SET_TO(a_rev[0], i[0]);
		SET_TO(a[1],     i[1]);
		SET_TO(a_rev[1], j[1]);

		SET_FROM(a[0],     i[0]);
		SET_FROM(a_rev[0], j[0]);
		SET_FROM(a[1],     j[1]);
		SET_FROM(a_rev[1], i[1]);

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

	zero_energy += E00;

	return e;
}
	void QPBO<REAL>::ComputeWeakPersistencies()
{
	if (stage == 0) return;

	Node* i;
	Node* j;
	Node* stack = NULL;
	int component;

	for (i=nodes[0]; i<node_last[0]; i++)
	{
		code_assert(i->label>=-1 && i->label<=1);

		Node* i1 = GetMate0(i);

		if (i->label >= 0)
		{
			i->dfs_parent = i;
			i1->dfs_parent = i1;
			i->region = i1->region = 0;
		}
		else
		{
			i->dfs_parent = i1->dfs_parent = NULL;
			i->region = i1->region = -1;
		}
	}

	// first DFS
	for (i=nodes[0]; i<node_last[1]; i++)
	{
		if (i == node_last[0]) i = nodes[1];
		if (i->dfs_parent) continue;

		// DFS starting from i
		i->dfs_parent = i;
		i->dfs_current = i->first;
		while ( 1 )
		{
			if (!i->dfs_current)
			{
				i->next = stack;
				stack = i;

				if (i->dfs_parent == i) break;
				i = i->dfs_parent;
				i->dfs_current = i->dfs_current->next;
				continue;
			}

			j = i->dfs_current->head;
			if (!(i->dfs_current->r_cap>0) || j->dfs_parent)
			{
				i->dfs_current = i->dfs_current->next;
				continue;
			}

			j->dfs_parent = i;
			i = j;
			i->dfs_current = i->first;
		}
	}

	// second DFS
	component = 0;
	while ( stack )
	{
		i = stack;
		stack = i->next;
		if (i->region > 0) continue;

		i->region = ++ component;
		i->dfs_parent = i;
		i->dfs_current = i->first;
		while ( 1 )
		{
			if (!i->dfs_current)
			{
				if (i->dfs_parent == i) break;
				i = i->dfs_parent;
				i->dfs_current = i->dfs_current->next;
				continue;
			}

			j = i->dfs_current->head;
			if (!(i->dfs_current->sister->r_cap>0) || j->region>=0)
			{
				i->dfs_current = i->dfs_current->next;
				continue;
			}

			j->dfs_parent = i;
			i = j;
			i->dfs_current = i->first;
			i->region = component;
		}
	}

	// assigning labels
	for (i=nodes[0]; i<node_last[0]; i++)
	{
		if (i->label < 0)
		{
			code_assert(i->region > 0);
			if      (i->region > GetMate0(i)->region) { i->label = 0; i->region = 0; }
			else if (i->region < GetMate0(i)->region) { i->label = 1; i->region = 0; }
		}
		else code_assert(i->region == 0);
	}
}