function [fgMask, Weights, Means, Variances] = detectForegroundAlgBuild...
    (I, learningRate, Weights, Means, Variances, ...
    classToUse, p)  %#codegen

% DETECTFOREGROUNDALGBUILD contains the algorithm used by 
% ForegroundDetector system object to compute a foreground mask using a 
% Gaussian Mixture Model (GMM). The object maintains the states of the GMM 
% model which are passed in and out of this function from mOutput callback 
% of the system object. This function is compiled into a mex file to speed 
% up the system object.
%
% fgMask: output logical mask where true represents foreground pixels
% 
% Weights, Means and Variances: states maintained outside of this function
%
% p: parameter structure that is expected to include the following fields:
%    numGaussians, varianceThreshold, minimumBackgroundRatio,
%    initialWeight, initialVariance, initialRank
%
% Copyright 2009 The MathWorks, Inc.
% $Revision: 1.1.6.2 $  $Date: 2010/11/17 13:14:13 $

%   References:
% [1]  P. Kaewtrakulpong, R. Bowden, "An Improved Adaptive Background 
%      Mixture Model for Realtime Tracking with Shadow Detection". In Proc. 
%      2nd European Workshop on Advanced Video Based Surveillance Systems, 
%      AVBS01, VIDEO BASED SURVEILLANCE SYSTEMS: Computer Vision and 
%      Distributed Processing (September 2001)
%
% [2]  Stauffer, C. and Grimson, W.E.L, "Adaptive Background Mixture Models 
%      for Real-Time Tracking". Computer Vision and Pattern Recognition, 
%      IEEE Computer Society Conference on, Vol. 2 (06 August 1999), 
%      pp. 2246-252 Vol. 2.

%% numChannels and frameSize of image I with singleton extensions
iSize = size(I);
if length(iSize) > 2
    numChannels = iSize(3);
else
    numChannels = 1;
end
if length(iSize) > 1
    frameSize = iSize(1:2);
else
    frameSize = [iSize(1), 1];
end

%% allocate per pixel buffers
states.w = zeros(1, p.numGaussians, classToUse);
states.m = zeros(numChannels, p.numGaussians, classToUse);
states.v = zeros(numChannels, p.numGaussians, classToUse);
states.rank = zeros(1, p.numGaussians, classToUse);
x = zeros(numChannels, 1, classToUse);

%% allocate output fgMask
fgMask = false(frameSize);

%% for each pixel
for row = 1:iSize(1)
    for col = 1:iSize(2)
        x(:) = I(row,col,:); % pixel at (row,col)
        
        %% get per pixel parameters from parameter buffers
        states.w(:)    = Weights(row,col,:);
        states.m(:, :) = Means(row,col,:,:);
        states.v(:, :) = Variances(row,col,:,:);
        
        %% initialize model update mechanism
        wSum = cast(0, classToUse);
        match = false;
        kCurr = p.numGaussians;
        kHit = cast(1, classToUse);
        
        %% for each Gaussian mode
        for k = 1:p.numGaussians
            kCurr = k;

            if (states.w(k) <= 0) % this mode needs to be initialized
                break;
            end

            wSum = wSum + states.w(k);

            %% compute distance of pixel from gaussian mean
            d = x(:) - states.m(:,k);
            dd = d.*d;
            normD = sum(dd);
            
            %% compute if the pixel belongs to kth mode
            match = normD<(p.varianceThreshold * sum(states.v(:,k)));

            if match
                %% update model parameters if match occured
                states = updateModelParameters(states, k, learningRate, ...
                    classToUse, d, dd);
                
                %% sort model parameters based on updated rank
                [states, kHit] = sortModelParameters(states, k, ...
                    classToUse);
                
                break; % winner takes all
            end
        end
        
        if (~match)
            kHit = kCurr;
            wSum = wSum + p.initialWeight - states.w(kHit);
            
            %% reinitialize lowest ranked gaussian mode
            states = initializeModelParameters(states, kHit, p, x);
        else
            %% accumulate remaining w(k)'s in wSum left out due to 'break'
            for k=kCurr+1:p.numGaussians
                wSum = wSum + states.w(k);
            end
        end
        
        %% compute background model
        % the first kBackground modes in the sorted model make up the
        % background model
        [states, kBackground] = getBackgroundModel(states, p, wSum, ...
            classToUse);
        
        %% compute fgMask
        % a pixel is a foreground pixel if it does not belong to the
        % background model computed above
        fgMask(row,col) = (kHit>kBackground);
        
        %% put the per pixel model parameters back into parameter buffers
        Weights(row,col,:)     = states.w;
        Means(row,col,:,:)     = states.m;
        Variances(row,col,:,:) = states.v;
    end
end

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% HELPER METHODS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% updateModelParameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function states = updateModelParameters(states, k, learningRate, ...
    classToUse, d, dd) %dd = d.*d;
% - updates model parameters for each of the kth gaussian mode
% - also updates the rank value of the kth gaussian mode
% - implements update equations derived from [2]
sumV = cast(0, classToUse);
for c = 1:size(states.m, 1)
    % mean
    states.m(c,k) = states.m(c,k) + learningRate * d(c);
    % variance
    states.v(c,k) = states.v(c,k) + learningRate * (dd(c) - states.v(c,k));
    sumV = sumV+states.v(c,k);
end
% weight
states.w(k)    = states.w(k) + learningRate * (1 - states.w(k));
% rank
states.rank(k) = states.w(k) / sqrt(sumV);
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% sortModelParameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [states, kHit] = sortModelParameters(states, k, classToUse)
% - sorts model parameters based on rank
kHit = cast(1, classToUse); % kHit returns as 1 when loop not entered
for ki = k:-1:2
    kHit = ki;
    if states.rank(ki-1) >= states.rank(ki) % ordered correctly
        break;                              % no need to swap
    end
    %% swap model parameters if rank ordering is wrong
    states = swapModelParameters(states, ki-1, ki);
    kHit = ki-1; % kHit changes after the swap
end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% initializeModelParameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function states = initializeModelParameters(states, k, p, x)
states.w(k) = p.initialWeight;
states.rank(k) = p.initialRank;
for c=1:size(states.m, 1)
    states.m(c,k) = x(c); % mean at current pixel
    states.v(c,k) = p.initialVariance;
end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% getBackgroundModel
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [states, kBackground] = getBackgroundModel(states, p, wSum, ...
    classToUse)
% - getBackgoroundModel updates the states while computing the number of
%   gaussians in the sorted mixture that make the background model
% - also scales weights and ranks by wSum
kBackground = cast(-1, classToUse); % set to invalid value
wScale = 1/wSum;
wSum = cast(0, classToUse);
for k=1:p.numGaussians
    states.w(k)    = states.w(k) * wScale;
    states.rank(k) = states.rank(k) * wScale;
    wSum = wSum + states.w(k); % accumulate weights of all modes
    if (wSum>=p.minimumBackgroundRatio ... 
            && kBackground<0) % if kBackground has not been set already
        kBackground = k;
    end
end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% swapModelParameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function  states = swapModelParameters(states, idx1, idx2)
% swap rank, w, m and v
states.rank = swap(states.rank, 1, idx1, idx2);
states.w    = swap(states.w, 1, idx1, idx2);
for c = 1:size(states.m, 1)
    states.m = swap(states.m, c, idx1, idx2);
    states.v = swap(states.v, c, idx1, idx2);
end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% swap
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function x = swap(x,c,idx1,idx2)
% swap element at (c,idx1) of 2D array x with element at (c,idx2)
temp = x(c,idx1);
x(c,idx1) = x(c,idx2);
x(c,idx2) = temp;
end
