function G = unrollsubgridflat(W,N,H,V,I,D)
% UNROLLGM Unroll a grid CRF for data creating a graphical model
%
% G = UNROLLGM(W,N,H,V) where W is the weight struct for a gcrf, N
% is the node data matrix, H and V are the horizontal and vertical
% edge data matrices, respectively. G is a grid graphical model struct.
    
            
% Jerod Weinman
% jerod@acm.org
  
%	$Id: unrollsubgridflat.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/>.

  if nargin<5 
	D = 1;
  end;
  
  L = size(W.node,2);
  
  sz = gridsize(N,H);

  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  % Calculate adjacency matrix

  K = nodenum(sz(1), sz(2));
  [E0,E1,nH] = gedges(K);
  
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  % Calculate new node numbers

  A = NaN*ones(size(N,1),1);
  A(I) = 1:length(I);
  
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  % Calculate node energies

  % Note: notes are ordered in the grid with their standard matlab indices
  
  % Perform standard multiplication (weight dot prod)
  P = N(I,:)*W.node(1:end-1,:) + ...
	  ones(length(I),1)*W.node(end,:);
  
  % Transpose for inferencer
  P = P';


  
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  % Calculate horizontal energies

  % H:  NxD
  % We: DxL^2
  % Uh: NxL^2
  

  
  Ix = I(:,ones(1,nH))';
  Ex = find(any(E0(1:nH,ones(1,length(I)))==Ix,2) & ...
            any(E1(1:nH,ones(1,length(I)))==Ix,2) );
  
  We = reshape(W.hEdge,[L*L size(W.hEdge,3)])';

  % Weight dot product
  Uh = H(Ex,:)*We(1:end-1,:) + ones(length(Ex),1)*We(end,:);
  
  E = [A(E0(Ex)) A(E1(Ex))];
  
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  % Calculate vertical energies

  % V: NxD
  % We: DxL^2
  % Uv: NxL^2

  Ix = I(:,ones(1,length(E0)-nH))';
  Ex = find(any(E0(nH+1:end,ones(1,length(I)))==Ix,2) & ...
            any(E1(nH+1:end,ones(1,length(I)))==Ix,2) );
  
  
  We = reshape(W.vEdge,[L*L size(W.vEdge,3)])';
	  
  % Weight dot product
  Uv = V(Ex,:)*We(1:end-1,:) + ones(length(Ex),1)*We(end,:);

  E = [E ; A(E0(nH+Ex)) A(E1(nH+Ex))];
  
  if D
	
	% Note: We could reduce the computation here by checking to see
    % if the CRF is symmetric and/or isotropic
	
	%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
	% Calculate default horizontal energies
	
	% Node Data: NxD
	% Wex: DxL^2
	% Un: NxL^2
	We1 = reshape(W.dhEdge(:,:,:,1), ...
				  [L*L size(W.dhEdge,3)])';
	
	We2 = reshape(W.dhEdge(:,:,:,2), ...
				  [L*L size(W.dhEdge,3)])';

	% Weight dot product
	Un1 = N*We1;
	Un2 = N*We2;
	

    % Rows and columns
	[R,M] = ind2sub([sz(1) sz(2)-1],1:size(H,1));

    % Node indices
    K1 = K(sub2ind(size(K),R,M))';
    K2 = K(sub2ind(size(K),R,M+1))';

    Ix = I(:,ones(1,length(K1)))';
    Ex = find(any(K1(:,ones(1,length(I)))==Ix,2) & ...
              any(K2(:,ones(1,length(I)))==Ix,2));
    
    
	Uh = Uh + Un1(K1(Ex),:) + Un2(K2(Ex),:);
	
	%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
	% Calculate default vertical energies

	% Node Data: NxD
	% Wex: DxL^2
	% Un: NxL^2
	We1 = reshape(W.dvEdge(:,:,:,1), ...
				  [L*L size(W.dvEdge,3)])';
	
	We2 = reshape(W.dvEdge(:,:,:,2), ...
				  [L*L size(W.dvEdge,3)])';

	% Weight dot product
	Un1 = N*We1;
	Un2 = N*We2;
	
    % Rows and columns
	[R,M] = ind2sub([sz(1)-1 sz(2)],1:size(V,1));

    % Node indices
    K1 = K(sub2ind(size(K),R,M))';
    K2 = K(sub2ind(size(K),R+1,M))';

    Ix = I(:,ones(1,length(K1)))';
    Ex = find(any(K1(:,ones(1,length(I)))==Ix,2) & ...
              any(K2(:,ones(1,length(I)))==Ix,2) );
  
    Uv = Uv + Un1(K1(Ex),:) + Un2(K2(Ex),:);
	
  end; % if D
  


  G = struct('numNodes', {length(I)}, ...
             'adjmat', {E}, ...
			 'numLabels', {L}, ...
			 'node',{P'}, ...
			 'edge',{[Uh ; Uv]} );
