% PURPOSE: compute minimizer of Tr(A*Y'*Y + B*Y) when Y size is K x T (time
% along HORIZONTAL dimension (second dimension in Matlab))
%
% INPUTS:
% - A       T x T real-valued matrix
% - B       T x K real-valued matrix (one should have K <= T)
%
% OUTPUTS:
% - Y       optimal alignment matrix of size K x T (K is number of
% occurrences, T is number of time steps)

function Y = optimizeTrYY_Y(A,B)

[T, K] = size(B);
if (T < K)
    error('Errors in dimensions: %i < %i (T < K)', T, K)
end


% compute integral image of A (along both dimensions)
Ai = cumsum(cumsum(A,1),2);

% compute image integral of B (along time dimension of length T)
Bi = cumsum(B,1);

% init dynamic program
Ind = zeros(K, T);  % Ind(k,t) = end time of penultimate segment of the best k-points segmentation of (1:t)
C = zeros(K, T);    % C(k,t) = contribution of the best k-points segmentation of (1:t)

% Cost of first segment
C(1,:) = diag(Ai).' + Bi(:,1).';

% Cost of further segments: dynamic programming
for k = 2:K % k = current segment number
    for t = 1:T % t = end time of current segment
        alpha = inf(1,t);
        for u = (k-1):(t-1) % u = end time of previous segment
            alpha(u) = C(k-1,u) +...        % contribution of previous segments 1:u
                        (Ai(u,u)+Ai(t,t)-Ai(u,t)-Ai(t,u)) + (Bi(t,k) - Bi(u,k));     % contribution of current segment u+1:t
        end
        [C(k,t), Ind(k,t)] = min(alpha);
    end
end

% Backtracking steps
Tc = T*ones(1, K);
for k = (K:-1:2)
    Tc(k-1) = Ind(k, Tc(k));
end

% Make Y alignement matrix out of segment end times Tc
Y = zeros(K, t);
first = 1;
for k = 1:K
    last = Tc(k);
    Y(k, first:last) = 1;
    first = last+1;
end

end