% PURPOSE: compute the K optimal change points in sequence feature K with
% the Gaussian assumption
%
% INPUTS:
% - X       features D x T (time is horizontal dimension D, feature dimensions D are vertical dimension)
% - K       number of segments (K-1 is the number of changepoints)
%
% OUTPUTS:
% - Tc    END times of each segment (including last time T) (size(Tc) = [1, K])

function Tc = optimizeKchangepoints(X,K)

T = size(X, 2); % number of time steps

if (T < K)
    error('Too few timesteps: %i < %i (T < K)', T, K)
end

% compute feature matrix
XX = X'*X;
% compute integral image
I = cumsum(cumsum(XX,1),2);

% init dynamic program
Ind = zeros(K, T);
C = zeros(K, T);
C(1,:) = diag(I).' ./ (1:T);
% for t =1:T
%     C(1,t) = sum(var(X(:,1:t), 0, 2)); 
% end

% compute dynamic program
for e = 2:K % e = current segment number
    for t = 1:T % t = end time of current segment
        alpha = -inf(1,t);
        for u = (e-1):(t-1) % u = end time of previous segment
            alpha(u) = C(e-1,u) +...        % contribution of previous segments [1, u]
                        (I(u,u)+I(t,t)-I(u,t)-I(t,u))/(t-u);     % contribution of current segment [u+1, t]   %+ sum(sum(XX(u+1:t,u+1:t)))/(t-u)
                                           %sum(var(X(:,u+1:t), 0, 2));

        end
        [C(e,t), Ind(e,t)] = max(alpha);
    end
end


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

%Ind

end


% % previous version
% 
% function [Tc,C] = changepoint2(X,K)
% 
% TT = size(X, 2); % number of time steps
% 
% % compute feature matrix
% XX = X'*X;
% % compute integral image
% I = cumsum(cumsum(XX,1),2);
% 
% % init dynamic program
% C = zeros([TT, TT, K]);
% C(1,:,1) = diag(I).' ./ (1:TT);
% 
% % compute dynamic program
% for e = 2:K
%     for t = 1:(TT-1)
%         % case of current t is a change point
%         M = max(C(1:t, t, e-1)); % contribution of previous segments up to time t
%         % default case
%         for u = (t+1):TT%(TT-K+e)
%             C(t+1, u,e) = M + (I(u,u)+I(t,t)-I(u,t)-I(t,u))/(u-t); % contribution of current segment e from time t+1 to u
%         end
%     end
% end
% 
% 
% % Backtracking steps
% Tc = TT*ones(1, K);
% for e = (K-1:-1:1)
%     e,Tc(e+1)
%     [~,Tc(e)] = max(C(1:(Tc(e+1)-1), Tc(e+1), e+1));
% end
% 
% %Tc = [1,Tc(1:end-1)];
% 
% end