function [] = gridbp()
% load image and compute the sample means and covariances
    Im = (1/255)*double(imread('flower.bmp'));
    Foreground = imread('foreground.bmp');
    Background = imread('background.bmp');
    Im1= Im(:,:,1);
    Im1= Im1(:);
    Im2= Im(:,:,2);
    Im2= Im2(:);
    Im3= Im(:,:,3);
    Im3= Im3(:);
    Fo = Foreground;
    Fo = Fo(:);
    Ba = Background;
    Ba = Ba(:);
    ForegroundSample = [Im1(find(Fo>0)),Im2(find(Fo>0)),Im3(find(Fo>0))];
    BackgroundSample = [Im1(find(Ba>0)),Im2(find(Ba>0)),Im3(find(Ba>0))];
    FM = mean(ForegroundSample)';   % Foreground sample mean
    FC = cov(ForegroundSample);     % Foreground sample covariance
    BM = mean(BackgroundSample)';   % Background sample mean
    BC = cov(BackgroundSample);     % Background sample covariance

% define parameters
    epsilon = 0.01;
    nrow = size(Im(:,:,1),1);
    ncol = size(Im(:,:,1),2);
    niter= 30;

% The following four matrices contain the messages (in log-likelihood format) 
% such that hhorright(i,j) = log(nu_{(i,j)->(i,j+1)}(+1)/nu_{(i,j)->(i,j+1)}(0))
    hhorright = zeros(nrow, ncol-1);  % hhorright(i,j) is the message from (i,j) to (i,j+1)
	hhorleft = zeros(nrow, ncol-1);   % hhorleft(i,j) is the message from (i,j+1) to (i,j)
	hvertup = zeros(nrow-1, ncol);    % hvertup(i,j) is the message from (i+1,j) to (i,j)
	hvertdown = zeros(nrow-1, ncol);  % hvertdown(i,j) is the message from (i,j) to (i+1,j)

% The following matrices store the updated messages, 
	hhorrightnew = zeros(nrow, ncol-1);
	hhorleftnew = zeros(nrow, ncol-1);
	hvertupnew = zeros(nrow-1, ncol);
	hvertdownnew = zeros(nrow-1, ncol);

% run parallel BP
	for t = 1:niter
		%looping over edges with 3 inputs
		for i = 2:(nrow-1)
			for j = 2:(ncol-1)
				hhorrightnew(i, j)  = F(epsilon,FM,FC,BM,BC,Im(i,j,:),[hhorright(i, j-1) hvertdown(i-1, j) hvertup(i, j)]);
                hvertdownnew(i, j)  = F(epsilon,FM,FC,BM,BC,Im(i,j,:),[hvertdown(i-1, j) hhorright(i, j-1) hhorleft(i,j)]);
				hhorleftnew(i, j-1) = F(epsilon,FM,FC,BM,BC,Im(i,j,:),[hhorleft(i, j) hvertdown(i-1, j) hvertup(i, j)]);
				hvertupnew(i-1, j)  = F(epsilon,FM,FC,BM,BC,Im(i,j,:),[hvertup(i, j) hhorright(i, j-1) hhorleft(i, j)]);
			end
        end
		%looping over edges with 2 inputs
		for i = 2:(nrow-1)
            hhorrightnew(i, 1)      = F(epsilon,FM,FC,BM,BC,Im(i, 1,:),     [hvertdown(i-1, 1) hvertup(i,1)]);
			hhorleftnew(i, ncol-1)  = F(epsilon,FM,FC,BM,BC,Im(i, ncol,:),  [hvertdown(i-1, ncol) hvertup(i,ncol)]);
			hvertdownnew(i, 1)      = F(epsilon,FM,FC,BM,BC,Im(i, 1,:),     [hvertdown(i-1, 1) hhorleft(i,1)]);
			hvertdownnew(i, ncol)   = F(epsilon,FM,FC,BM,BC,Im(i, ncol,:),  [hvertdown(i-1, ncol) hhorright(i-1,ncol-1)]);
			hvertupnew(i-1, 1)      = F(epsilon,FM,FC,BM,BC,Im(i, 1,:),     [hvertup(i, 1)   hhorleft(i,1)]);
			hvertupnew(i-1, ncol)   = F(epsilon,FM,FC,BM,BC,Im(i, ncol,:),  [hvertup(i,ncol)    hhorright(i,ncol-1)]);
        end
		for i = 2:(ncol-1)
			hvertupnew(nrow-1, i)   = F(epsilon,FM,FC,BM,BC,Im(nrow, i,:), [hhorright(nrow, i-1) hhorleft(nrow, i)]);
			hvertdownnew(1, i)      = F(epsilon,FM,FC,BM,BC,Im(1, i,:),    [hhorright(1, i-1) hhorleft(1, i)]);
			hhorrightnew(1, i)      = F(epsilon,FM,FC,BM,BC,Im(1, i,:),    [hhorright(1, i-1) hvertup(1, i)]);
			hhorrightnew(nrow, i)   = F(epsilon,FM,FC,BM,BC,Im(nrow, i,:), [hhorright(nrow, i-1) hvertdown(nrow-1, i)]);
			hhorleftnew(1,i-1)      = F(epsilon,FM,FC,BM,BC,Im(1, i,:),    [hhorleft(1, i)    hvertup(1, i)]);
			hhorleftnew(nrow,i-1)   = F(epsilon,FM,FC,BM,BC,Im(nrow, i,:), [hhorleft(nrow, i)    hvertdown(nrow-1, i)]);
        end

		%Edges with single input
		hvertdownnew(1, 1)       = F(epsilon,FM,FC,BM,BC,Im(1, 1,:),     [hhorleft(1, 1)]);
		hvertdownnew(1, ncol)    = F(epsilon,FM,FC,BM,BC,Im(1,ncol,:),   [hhorright(1,ncol-1)]);
        hvertupnew(nrow-1, 1)    = F(epsilon,FM,FC,BM,BC,Im(nrow,1,:),   [hhorleft(nrow, 1)]);
        hvertupnew(nrow-1, ncol) = F(epsilon,FM,FC,BM,BC,Im(nrow,ncol,:),[hhorright(nrow, ncol-1)]);

        hhorrightnew(1, 1)        = F(epsilon,FM,FC,BM,BC,Im(1, 1,:),      [hvertup(1, 1)]);
        hhorrightnew(nrow, 1)     = F(epsilon,FM,FC,BM,BC,Im(nrow, 1,:),   [hvertdown(nrow-1, 1)]);
        hhorleftnew(1, ncol-1)    = F(epsilon,FM,FC,BM,BC,Im(1, ncol,:),   [hvertup(1, ncol-1)]);
        hhorleftnew(nrow, ncol-1) = F(epsilon,FM,FC,BM,BC,Im(nrow, ncol,:),[hvertdown(nrow-1, ncol)]);
            
        % store the new messages as current messages
		hvertdown = hvertdownnew;
		hvertup   = hvertupnew;
		hhorleft  = hhorleftnew;
		hhorright = hhorrightnew;
    end
% compute log likelihood ratio of the marginals 
		% nodes of degree 4
		for i = 2:(nrow-1)
			for j = 2:(ncol-1)
				llr(i, j) = F(epsilon,FM,FC,BM,BC,Im(i,j,:),[hhorright(i, j-1) hvertdown(i-1, j) hvertup(i, j) hhorleft(i, j)]);
			end
        end
		% nodes of degree 3
		for i = 2:(nrow-1)
            llr(i,1)    = F(epsilon,FM,FC,BM,BC,Im(i, 1,:),   [hvertdown(i-1, 1) hvertup(i,1) hhorleft(i,1) ]);
            llr(i,ncol) = F(epsilon,FM,FC,BM,BC,Im(i, ncol,:),[hvertdown(i-1, ncol) hvertup(i,ncol) hhorright(i,ncol-1) ]);
        end
		for j = 2:(ncol-1)
            llr(1,j)    = F(epsilon,FM,FC,BM,BC,Im(1, j,:),   [hhorright(1, j-1) hhorleft(1, j) hvertup(1, j)]); 
            llr(nrow,j) = F(epsilon,FM,FC,BM,BC,Im(nrow, j,:),[hhorright(nrow, j-1) hhorleft(nrow, j) hvertdown(nrow-1, j)]);
        end
		% nodes of degree 2
		llr(1, 1)       = F(epsilon,FM,FC,BM,BC,Im(1, 1,:),     [hhorleft(1, 1) hvertup(1, 1)]);
		llr(1, ncol)    = F(epsilon,FM,FC,BM,BC,Im(1,ncol,:),   [hhorright(1,ncol-1) hvertup(1, ncol-1)]);
        llr(nrow, 1)    = F(epsilon,FM,FC,BM,BC,Im(nrow,1,:),   [hhorleft(nrow, 1) hvertdown(nrow-1, 1)]);
        llr(nrow, ncol) = F(epsilon,FM,FC,BM,BC,Im(nrow,ncol,:),[hhorright(nrow, ncol-1) hvertdown(nrow-1, ncol)]);

% convert from log-likelihoods to probabilities
        m = (exp(1).^(llr))./(1+exp(1).^(llr));
% plot the original image        
        figure();imagesc(Im);
% plot the expectation
    figure();imagesc(m);
end

function [out] = pair(x1,x2)
%**************************************************************************
% define pairwise potential function such that 
% pair(x1,x2) = psi(x1,x2)
%**************************************************************************
    if (x1==x2)
        out = 0.9;
    else
        out = 0.1;
    end
end

function [out] = single(x,y,epsilon,ForegroundMean,ForegroundVar,BackgroundMean,BackgroundVar)
%**************************************************************************
% define singleton potential function such that 
% single(x,y) = phi(x,y)
%**************************************************************************
    if (x==0)
        out = epsilon + (1/((2*pi)^(3/2)*sqrt(det(BackgroundVar))))*exp(-0.5*(y-BackgroundMean)'*inv(BackgroundVar)*(y-BackgroundMean));
    else
        out = epsilon + (1/((2*pi)^(3/2)*sqrt(det(ForegroundVar))))*exp(-0.5*(y-ForegroundMean)'*inv(ForegroundVar)*(y-ForegroundMean));
    end
end

function [MessageOut] = F(epsilon,FM,FC,BM,BC,y,MessageIn)
%**************************************************************************
% define BP update function that takes a set of messages as inputs 
% and ouputs the updated message according to the BP update rule 
% F(y,nu=[nu1 nu2 ...]) = log(phi(1,y)/phi(0,y)) + sum{log((psi(1,1)e^nu(i)+psi(1,0))/(psi(0,1)e^nu(i)+psi(0,0)))}
%**************************************************************************
    A=[];
    for i=1:length(MessageIn)
        A = [A; (pair(1,1)*exp(MessageIn(i))+pair(1,0))/(pair(0,1)*exp(MessageIn(i))+pair(0,0));];
    end
    y=y(:);
    MessageOut = log(single(1,y,epsilon,FM,FC,BM,BC)/single(0,y,epsilon,FM,FC,BM,BC)) + sum(log(A));
end


