%% Time-Varying Systems and Computations
% Klaus Diepold, Sven Gronauer, Simon Schumann, Zongxie Chen
%
%% Hankel Norm Reduction
% In this script, we approximate a system with a system of lower degree (lower complexity). We do this with balanced hankel factorizations by truncating the singular values.
%% 1. Cut out Hankel maps
% We start out with the example matrix from the lecture script.

T = [ 1     0     0     0     0     0;  
      0.8   0.9   0     0     0     0;
      0.2   0.6   0.8   0     0     0;
      0.05  0.24  0.5   0.7   0     0;
      0.013 0.096 0.25  0.4   0.6   0;
      0.003 0.038 0.125 0.240 0.3   0.5 ]

% We first cut out hankel maps and factorize them into observability and reachability matrices.

Obs = {};
Con = {};
singular_values = [];
K = size(T,2);
for i=1:K-1
    H = fliplr(T(i+1:end, 1:i));
    fprintf("H{%d} = ", i)
    disp(H)
    [U,S,V] = svd(H);
    r = min(size(S));
    singular_values(1:r,i) = diag(S(1:r,1:r));
    Obs{i} = U * sqrt(S);
    Con{i} = sqrt(S) * V';
end

% Now, we plot table of singular values for each timestep like in lecture script:

array2table(singular_values, 'VariableNames', compose("H{%d}", 1:K-1))

% Then, we define a threshold value for the singular values and truncate the observability and reachability matrices of each timestep accordingly.

epsilon = 0.01;
singular_values_red = [];
for i=1:K-1
    r = sum(singular_values(:,i) > 0);
    r_red = sum(singular_values(:,i) > epsilon);
    fprintf("Original rank: %d, reduced rank: %d\n", r, r_red)
    Obs_red{i} = Obs{i}(:,1:r_red);
    Con_red{i} = Con{i}(1:r_red,:);
    singular_values_red(1:r_red,i) = singular_values(1:r_red,i);
end

% Finally, we plot table with reduced singular values of hankel matrices:

array2table(singular_values_red, 'VariableNames', compose("H_red{%d}", 1:K-1))

% What would be a good way to determine epsilon?
% Can you think of other ways how to decide which singular values are truncated?
%
%% 2. Find a realization for the reduced system
% One way for finding realizations from the observability and reachability matrices is given in the script as
% A_{k-1} =\overset{\gets }{C_k } C_{k-1}^{\dagger}
% B_{k-1} =\textrm{first}\;\textrm{column}\;\textrm{of}\;\textrm{Controlability}\;\textrm{at}\;\textrm{time}\;k
% C_{k-1} =\textrm{first}\;\textrm{row}\;\textrm{of}\;Observability at time k

% For timestep 0 Controlability is empty, there are no past inputs
% to be mapped into internal states
Con_prev = zeros(0,0);
% For timestep 0 Observability is empty, there are no internal states
% which have to be mapped into future outputs
Obs_prev = zeros(1,0);
% Finding A's, B's and C's
for i = 1:K
    if i > 1
        Con_prev = Con_red{i-1};
        Obs_prev = Obs_red{i-1};
    end
    % For last timestep Controlability is empty, the past
    % K inputs are mapped into the "void"
    Con_present = zeros(0,K);
    if i < K
        Con_present = Con_red{i};
    end
    A{i} = Con_present(:,2:end)*pinv(Con_prev);
    B{i} = Con_present(:,1);
    C{i} = Obs_prev(1,:);
end
% D's are just (block-)diagonal elements
D = num2cell(diag(T), 2)';

% Let's reconstruct now the transfer operator and compare it with the original one.

A_diag = blkdiag(A{:});
B_diag = blkdiag(B{:});
C_diag = blkdiag(C{:});
D_diag = blkdiag(D{:});

% Structure of the  truncated system:

figure
spy([A_diag, B_diag; C_diag, D_diag])

% To do that we define the causal shift operator Z and an identity matrix I of approatiate size. Then we compute the transfer operator (matrix) T.

shifted_by = size(A{1},2);
Z = diag(ones(1,size(A_diag,1)-shifted_by),-shifted_by);
I = eye(size(A_diag));

% T=D_{\textrm{diag}} +C_{\textrm{diag}} \cdot {\left(I-Z\cdot A_{\textrm{diag}} \right)}^{-1} \cdot Z\cdot B_{\textrm{diag}}

T_red = D_diag + C_diag*(I - Z*A_diag)^-1*Z*B_diag;

% Compare the transfer operators:

err = norm(T - T_red, "fro")