clear all

%% EXERCISE SESSION 5 - PROBLEM 2 
%  PARTS (c) and (d) SOLUTIONS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


% Model parameters

A = [1.75 0.8 
     -0.95 0];

B = [1
     0];
 
    
%% PART (c) SOLUTIONS

% Two t parameters
t1 = 3;
t2 = 10;

% Calculate Reachability Gramians and their Inverses
G1 = zeros(size(A));
G2 = G1;

for i = 1:t1
   G1 = G1 + A^(i-1)*(B*B')*(A^(i-1))'; 
end

for i = 1:t2
   G2 = G2 + A^(i-1)*(B*B')*(A^(i-1))'; 
end

iG1 = inv(G1);
iG2 = inv(G2);

% Check of the minimum energy condition 
xbar1 = [];
xbar2 = [];

for x1 = -10:1e-2:10
    for x2 = -10:1e-2:10

        if [x1 x2]*iG1*[x1;x2] <= 1
            xbar1(1:2,end+1) = [x1;x2];
        end
        
        if [x1 x2]*iG2*[x1;x2] <= 1
            xbar2(1:2,end+1) = [x1;x2];
        end
        
    end
end

% Plot the states that are eligible
figure(1);
plot(xbar1(1,:),xbar1(2,:),'.');
grid on;
set(gca,'FontSize',15,'fontname','times','ylim',[-10, 10], 'xlim',[-10, 10])
xlabel('$$\mathbf{x_1}$$','Interpreter','Latex');
ylabel('$$\mathbf{x_2}$$','Interpreter','Latex');
title('$$\mathbf{\{\bar{x} \in R^n: \mathcal{E}_{min}(\bar{x},3) \leq 1\}}$$','Interpreter','Latex');

figure(2);
plot(xbar2(1,:),xbar2(2,:),'.');
grid on;
set(gca,'FontSize',15,'fontname','times','ylim',[-10, 10], 'xlim',[-10, 10])
xlabel('$$\mathbf{x_1}$$','Interpreter','Latex');
ylabel('$$\mathbf{x_2}$$','Interpreter','Latex');
title('$$\mathbf{\{\bar{x} \in R^n: \mathcal{E}_{min}(\bar{x},10) \leq 1\}}$$','Interpreter','Latex');




%% PART (d) SOLUTIONS

% Start from t=2 (because it is not possible to reach xbar in one step)
tmin = 2;
tmax = 25;

xbar = [1; 1];

% Define reachability Gramian 
G = B*B';

Emin = zeros(tmax-tmin+1,1);

% Iteration for updating the reachability Gramian and minimum energy
for t = tmin:tmax
    
    G = G + A^(t-1)*(B*B')*(A^(t-1))';
    iG = inv(G);
    Emin(t-tmin+1) = xbar'*iG*xbar;
    
end

% Plot the minimum energy vs. t
figure(3);
plot(tmin:tmax, Emin, 'o-', 'LineWidth', 2);
set(gca, 'FontSize',15,'fontname','times','xlim',[0, tmax+1],'ylim',[0,max(Emin)+1]);
xlabel('$$\mathbf{t}$$','Interpreter','Latex');
ylabel('$$\mathbf{\mathcal{E}_{min}(t)}$$','Interpreter','Latex');
grid on;