function [Output] = CEM(Image,Library)

% Constrained Energy Minimization
% Silas J. Leavesley, PhD
% University of South Alabama
% Last Updated: 2/7/2017

% To visualize output data:
% figure(1)
% for i = 1:length(Output.data(1,1,:))
%   subplot(1,length(Output.data(1,1,:)),i)
%   imagesc(Output.data(:,:,i))
% end
% colormap(gray) % or any other colormap, as desired

tic

Image=double(Image);

[L M N]=size(Image);
[O P]=size(Library);
Image_Reshaped=double((reshape(Image,(L*M),N))');
q=L*M;
sum=zeros(N,N);

for i=1:q
    product=Image_Reshaped(:,i)*(Image_Reshaped(:,i)');
    sum=sum+product;
end

R=sum/q;
d = Library;
W = inv(R)*d*inv(transpose(d)*inv(R)*d); 

for i = 1:P
    for j = 1:q
        CEM(j,i) = transpose(W(:,i))*Image_Reshaped(:,j);
    end
end

Output.data=reshape(CEM,L,M,P);
toc