%% Time-Varying Systems and Computations
% Klaus Diepold, Sven Gronauer, Simon Schumann
%
%% 1. State-Space Arithmetics
% Given are two causal SISO systems \Sigma_1 and \Sigma_2. They are stored in MATLAB struct variables.

% First system
state_dims_1 = [0, 1, 2, 3, 2, 3, 2, 1, 0];
Sigma_1 = struct;

% Second system
state_dims_2 = [0, 1, 2, 3, 4, 3, 2, 1, 0];
Sigma_2 = struct;

% We initialize the system realizations randomly:

K = length(state_dims_1) - 1; % Number of stages
for i = 1:K
    % System 1
    Sigma_1.A{i} = rand(state_dims_1(i+1),state_dims_1(i));
    Sigma_1.B{i} = rand(state_dims_1(i+1), 1);
    Sigma_1.C{i} = rand(1, state_dims_1(i));
    Sigma_1.D{i} = rand(1,1);
    
    % System 2
    Sigma_2.A{i} = rand(state_dims_2(i+1),state_dims_2(i));
    Sigma_2.B{i} = rand(state_dims_2(i+1), 1);
    Sigma_2.C{i} = rand(1, state_dims_2(i));
    Sigma_2.D{i} = rand(1,1);
end

% To compare the results obtained with state-space methods and operator matrices, we generate T_1 and T_2.

% Matrix System 1
A_1_diag = blkdiag(Sigma_1.A{:}); B_1_diag = blkdiag(Sigma_1.B{:});
C_1_diag = blkdiag(Sigma_1.C{:}); D_1_diag = blkdiag(Sigma_1.D{:});
shifted_by = size(Sigma_1.A{1},2);
Z = diag(ones(1,size(A_1_diag,1)-shifted_by),-shifted_by);
I = eye(size(A_1_diag)); 
T_1 = D_1_diag + C_1_diag * pinv(I - Z*A_1_diag) * Z * B_1_diag;

% Matrix System 2
A_2_diag = blkdiag(Sigma_2.A{:}); B_2_diag = blkdiag(Sigma_2.B{:});
C_2_diag = blkdiag(Sigma_2.C{:}); D_2_diag = blkdiag(Sigma_2.D{:});
shifted_by = size(Sigma_2.A{1},2);
Z = diag(ones(1,size(A_2_diag,1)-shifted_by),-shifted_by);
I = eye(size(A_2_diag)); 
T_2 = D_2_diag + C_2_diag * pinv(I - Z*A_2_diag) * Z * B_2_diag;

% Now, we take a look at the diagonal representation of the realizations:

figure
subplot(1,2,1)
spy([A_1_diag, B_1_diag; C_1_diag, D_1_diag])
title("Realization 1")
subplot(1,2,2)
spy([A_2_diag, B_2_diag; C_2_diag, D_2_diag])
title("Realization 2")

% Finally, we construct the realtization for the sum of system 1 and system 2 as described in the lecture script:
% \Sigma_S =\left\lbrack \begin{array}{cccc}
% A_1  & 0 & \left|\right. & B_1 \\
% 0 & A_2  & \left|\right. & B_2 \\
% - & - & \left|\right. & -\\
% C_1  & C_2  & \left|\right. & D_1 +D_2
% \end{array}\right\rbrack

Sigma_S = struct;
for i = 1:length(Sigma_1.A)
    Sigma_S.A{i} = blkdiag(Sigma_1.A{i}, Sigma_2.A{i});
    Sigma_S.B{i} = [Sigma_1.B{i}; Sigma_2.B{i}];
    Sigma_S.C{i} = [Sigma_1.C{i}, Sigma_2.C{i}];
    Sigma_S.D{i} = Sigma_1.D{i} + Sigma_2.D{i};
end

% Which conditions (e.g. regarding the shapes of the A, B, C and D matrices) must be fulfilled that this formula can be used?
%
%% 2. Computing the Matrix Vector Product
% Now, we compute the output of \Sigma_S if we apply a vector u:

u = randn(size(D_1_diag,2),1);
x = {zeros(size(Sigma_S.A{1},2),1)}; % Initial state value
y = {};
for i = 1:length(Sigma_S.A)
    % Calculate next state
    x{i+1} = Sigma_S.A{i}*x{i} + Sigma_S.B{i}*u(i);
    % Calculate output of current stage
    y{i} = Sigma_S.C{i}*x{i} + Sigma_S.D{i}*u(i);
end
y_ss = vertcat(y{:});

% Then, we compute the result using the operator matrices T_1 and T_2

T_S = T_1 + T_2;
y_T = T_S * u;

% and compare the results:

mse = mean((y_ss - y_T) .^ 2)

%
%% 3. Non minimal state-space
% In this section, we add two identical systems (e.g. System 1 and System 1) and inspect the resulting state dimensions for each step.

% State space realization of T_1 + T_1
Sigma_1p1 = struct;
for i = 1:length(Sigma_1.A)
    Sigma_1p1.A{i} = blkdiag(Sigma_1.A{i}, Sigma_1.A{i});
    Sigma_1p1.B{i} = [Sigma_1.B{i}; Sigma_1.B{i}];
    Sigma_1p1.C{i} = [Sigma_1.C{i}, Sigma_1.C{i}];
    Sigma_1p1.D{i} = Sigma_1.D{i} + Sigma_1.D{i};
end

% First, we calculate the operator matrix:

% Matrix of System 1 + System 1
A_1p1_diag = blkdiag(Sigma_1p1.A{:}); B_1p1_diag = blkdiag(Sigma_1p1.B{:});
C_1p1_diag = blkdiag(Sigma_1p1.C{:}); D_1p1_diag = blkdiag(Sigma_1p1.D{:});
shifted_by = size(Sigma_1p1.A{1},2);
Z = diag(ones(1,size(A_1p1_diag,1)-shifted_by),-shifted_by);
I = eye(size(A_1p1_diag)); 
T_1p1 = D_1p1_diag + C_1p1_diag * pinv(I - Z*A_1p1_diag) * Z * B_1p1_diag;

% Having the diagonal representations at hand, we plot how the system "looks" like:

figure
spy([A_1p1_diag, B_1p1_diag; C_1p1_diag, D_1p1_diag])
title("System 1 + System 1")

% Here, we confirm that the resulting matrix is the same as two times the matrix of System 1:

err = norm(T_1p1 - 2*T_1, 'fro')

% Then, we check the output size of state-space:

state_dims = [];
for i=1:length(Sigma_1p1.A)
    state_dims(end+1) = size(Sigma_1p1.A{i},2);
end

figure
plot(state_dims)
xlabel('Stage')
ylabel('State Dimension')

% Could we maybe find a realization which corresponds to the same system but uses less internal states?
% To do that, we check out the following alternative system:

Sigma_1p1_alt = struct;
for i = 1:length(Sigma_1.A)
    Sigma_1p1_alt.A{i} = Sigma_1.A{i};
    Sigma_1p1_alt.B{i} = Sigma_1.B{i} * 2;
    Sigma_1p1_alt.C{i} = Sigma_1.C{i};
    Sigma_1p1_alt.D{i} = Sigma_1.D{i} * 2;
end

% Does this realization correspond to the same system we obtained by adding System 1 with itself?
% To check this, we calculate and compare the transfer operator matrices:

% Matrix of alternative for System 1 + System 1
A_1p1_diag_alt = blkdiag(Sigma_1p1_alt.A{:}); 
B_1p1_diag_alt = blkdiag(Sigma_1p1_alt.B{:});
C_1p1_diag_alt = blkdiag(Sigma_1p1_alt.C{:}); 
D_1p1_diag_alt = blkdiag(Sigma_1p1_alt.D{:});
shifted_by = size(Sigma_1p1_alt.A{1},2);
Z = diag(ones(1,size(A_1p1_diag_alt,1)-shifted_by),-shifted_by);
I = eye(size(A_1p1_diag_alt)); 
T_1p1_alt = D_1p1_diag_alt + ...
    C_1p1_diag_alt * pinv(I - Z*A_1p1_diag_alt) * Z * B_1p1_diag_alt;
%Compare the two transfer operator matrices and find out that they are identical (up to nummerical precision):
norm(T_1p1 - T_1p1_alt, 'fro')

% Finally, we varify that the alternative realization has less internal states and is therefore more efficient in terms of memory and computation:

state_dims = [];
state_dims_alt = [];
for i=1:length(Sigma_1p1.A)
    state_dims(end+1) = size(Sigma_1p1.A{i},2);
    state_dims_alt(end+1) = size(Sigma_1p1_alt.A{i},2);
end

figure
hold on
plot(state_dims)
plot(state_dims_alt)
legend(["state dim.", "alt. state dim."])
xlabel('Stage')
ylabel('State Dimension')

% Can we always find a realization which has smaller state dimension than the sum over state dimensions of the two given realizations?