%% Time-Varying Systems and Computations
% Klaus Diepold, Sven Gronauer, Simon Schumann
%
%% 1. Conversion from State Space Matrices to Dense Matrix
% Let's first create a state-space system with low local state dimensions such that the state "grows" from 1 to the maximum at the first stages and  it "shrinks" back to 1 at the last stages.

state_dims_max = 2;
state_dims = [
    1:state_dims_max, ...
    ones(1,22)*state_dims_max, ...
    state_dims_max:-1:1 ];

% To see the evolution of state dimensionality, we plot the dimension per stage:

figure
plot(state_dims)
xlabel("Stage")
ylabel("Dimension")

% Now, using the given state dimension for each time step, we create the system matrices of a causal system. (Note that the system is a single-input-single-output system, each step involves one input value u_i and one output value y_i)
% \left\lbrack \begin{array}{c}
% x_{i+1} \\
% y_i
% \end{array}\right\rbrack =\left\lbrack \begin{array}{cc}
% A_i  & B_i \\
% C_i  & D_i
% \end{array}\right\rbrack \left\lbrack \begin{array}{c}
% x_i \\
% u_i
% \end{array}\right\rbrack

A = {}; B = {}; C = {}; D = {};
K = length(state_dims) - 1; % Number of stages
for i = 1:K
    A{i} = randn(state_dims(i+1), state_dims(i));
    B{i} = randn(state_dims(i+1), 1);
    C{i} = randn(1, state_dims(i));
    D{i} = randn(1,1);
end

% In order to find the matrix representation of the system, we generate the diagonal representation of the system matrices

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

% and visualize their structure:

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

% Now, we define the causal shift operator Z and an identity matrix I of approatiate size which will be needed to compute the transfer operator (matrix) T.

Z = diag(ones(1,size(A_diag,1)-1),-1);
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 = D_diag + C_diag*(I - Z*A_diag)^-1*Z*B_diag;

% Finally, we verify that T corresponds to a causal system. (Has to be lower triangular matrix)

figure
spy(abs(T) > 1e-10)

% What happens if our T operator is not lower triangular?
%
%% 2. Computation of the Matrix Vector Product using the Dense Matrix
% We first set up an input vector u. Then, we multiply T with this input vector and calculate the number of floating point operations involved.

u = randn(K,1);
y_dense = T*u;
% Number of operations for Lower Triangular * Vector = N^2-1
ops_dense = K^2 - 1;

%
%% 3. Computation of the Matrix Vector Product in State Space
% We calculate the result vector y using the sequential computation schema in state-space and track the total number of floating point operations neccessary.

x = {0}; % Initial state value
y = {};
ops_ss = 0;
for i = 1:K
    % Calculate next state
    x{i+1} = A{i}*x{i} + B{i}*u(i);    
    % Adding number of operations for state calculation
    ops_ss = ops_ss + size(A{i},1) * (2*size(A{i},2) - 1) ...
        + size(B{i},1) * (2*size(B{i},2) - 1) ... 
        + numel(x{i+1});
    % Calculate output of current stage
    y{i} = C{i}*x{i} + D{i}*u(i);
    % Adding number of operations for output calculation
    ops_ss = ops_ss + size(C{i},1) * (2*size(C{i},2) - 1) ...
        + size(D{i},1) * (2*size(D{i},2) - 1) ... 
        + numel(y{i});
end
y_ss = vertcat(y{:});

% In which cases does the computation of a matrix vector product in State Space should be favored over computing the result in operator space?
%
%% 4. Comparison of the results from vector products
% Now, we compute the mean squared error between state-space and dense matrix solution:

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

% Why is the mse not 0 here?
%
%% 5. Comparison of the number of operations needed for both of the compuatation schemes

figure
bar(categorical({'state-space', 'dense'}),[ops_ss, ops_dense])
ylabel("flops")

% Which version needs less computations? Try out also for different state dimensions and number of stages.