function Y = icm(C,X,Y0)
% ICM Finds a conditional modal configuration of data instance X
%
% [Y,F] = ICM(C,X) where C is the CRF, F is an exit flag, and X is
% the data instance. Y are the node labels.
%
% [Y,F] = ICM(C,X,Y0) where Y0 is the initial configuration

% GCRF.ICM


% Jerod Weinman
% jerod@acm.org
    
% $Id: icm.m 1 2009-05-07 13:41:00Z weinman $
 
% Jerod Weinman
% jerod@acm.org
% Copyright 2004, 2008, 2009
 
%    This file is part of Matlab GridCRF.
%
%    Matlab GridCRF is free software: you can redistribute it and/or modify
%    it under the terms of the GNU General Public License as published by
%    the Free Software Foundation, either version 3 of the License, or
%    (at your option) any later version.
%
%    Matlab GridCRF is distributed in the hope that it will be useful,
%    but WITHOUT ANY WARRANTY; without even the implied warranty of
%    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%    GNU General Public License for more details.
%
%    You should have received a copy of the GNU General Public License
%    along with Matlab GridCRF.  If not, see <http://www.gnu.org/licenses/>.
    
  M = unrollgm(C,X.node,X.hEdge,X.vEdge);
  
  conditionalMode(M);
  
  E = M.adjmat;
  
  
  % Build neighbor lists
  for n=1:M.numNodes
	N{n} = neighbors(E,n);
  end;
  
  if nargin<3
	% Initialize at local max
	mxH = max(M.nodeEnergies);
	
	[Y0,nn] = find(mxH(ones(1,M.numLabels),:)==M.nodeEnergies);
	
	Y0(nn) = Y0; % put them in the right order
	Y0 = Y0(1:M.numNodes)';
	
  elseif length(Y0)~=M.numNodes
	error('Initial labeling has incorrect number of nodes')
  elseif min(Y0)<1 | max(Y0)>size(M.nodeEnergies,1)
	error('Initial labeling has invalid label(s)');
  end;
	
  
  [Y,F] = iterate(Y0,N);
  
  if F==2
	% find configuration in the orbit with maximal energy
	
	U = NaN;
	
	for c=1:size(Y,1)

	  Uc = energy(M,Y(c,:));
	  
	  if isnan(U) | (Uc>U)
		U = Uc;
		m = c;
	  end;
	end;
	
	Y = Y(m,:);
  end;
	  
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function P = conditionalMode(N,G,L)
% CONDITIONALMODE Conditional mode of a node given neighbors' labels
%
% P = conditionalMode(N,G,L) where N is the node, G is its list
% of neighbors, and L is the list of their values
  
  persistent M

  if nargin==1
	M = N;
	return;
  end;
  
  % FIX ME
  G0 = min(N,G);
  G1 = max(N,G);
  
  U = M.nodeEnergies(:,N);
  
  for n=1:length(G0)
	U = U + M.edgeEnergies{G0(n),G1(n)}(:,L(n));
  end;


  P = find(U==max(U));
  P = P(1);
  
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [Y,F] = iterate(Y0,N,I)
% ITERATE Iteratively calculate conditional modes
  
  MAX_ITER = 1000;
  
	% History
	H = zeros(MAX_ITER,length(Y0));
	
	% iterate for convergence
	Y1 = -ones(size(Y0));
	
	iter = 1;
	
	while iter <= MAX_ITER
	  
	  H(iter,:) = Y0;
	  
	  for n=1:length(N)
		Y1(n) = conditionalMode(n,N{n},Y0(N{n}));
	  end;

	  if all(Y1==Y0)
		disp(sprintf('icm converged in %d iterations',iter));
		Y = Y1;
		F = 0;
		return;
	  end;
	  
	  % hist diff: is true if any label differs
	  hd = any(Y1(ones(1,iter-1),:) ~= H(1:iter-1,:),2);
	  
	  if any(hd==0) % if no diff
		
		li = find(hd==0);
		
		disp(sprintf('icm caught in orbit of length %d',(iter+1-li)));
		
		Y = H(li:iter,:);
		F = 2;
		
		return;
	  end;
	  
	  iter = iter + 1;
	  Y0 = Y1;
	end;
	
  
	Y = Y1;
	
	if (iter==MAX_ITER)
	  disp('icm failed to converge');
	  F = 1;
	else
	  F = 0;
	end;
	

	
