clear;

%% system matrices

A = [0.13, -0.93;
     0.57, 0];
 
B = [17.2;
      0.0];

C = [0, 1];


%% Solve Ackermann equation

W = [B A*B];
W_tilde = inv([1 -0.13;
               0   1]);
       
a = [-0.13 0.5301];
p = [0.3 0.02];

K_1 = (p-a)*W_tilde*inv(W);  %% better to not use inverses but \


%% desired eigenvalues
eigvals = [-0.1, -0.2];

% eigvals = [-0.1+0.5i, -0.1-0.5i];

%% Use matlab functions for eigenvalue assignment

K_2 = place(A, B, eigvals);
K_3 = acker(A, B, eigvals);  %% numerically instable 

%% 

k_f = 1./(C*inv(A-B*K_2)*B);

%% Closed loop system

% equilibirum
H_e = 20.6;
L_e = 29.5;

% closed loop system matrices
A_cl = A-B*K_2;
B_cl = B*k_f;

sys_cl = ss(A_cl, B_cl, C, 0);

tspan = 0:0.5:100;

L_r = 30; %% reference value

r = L_r - L_e;

u = r*ones(size(tspan)); 

x0s= [20, 20;
      40, 10;
      10, 40];

figure(1);

for i=1:3
    subplot(3,1,i); hold on;
    
    x0 = x0s(i,:);
    [y,t,x] = lsim(sys_cl,u,tspan, x0);

    plot(t, x(:,1)+H_e, '-');
    plot(t, x(:,2)+L_e, '-');    
    xlabel('time (years)');
    ylabel('number of hares/lynxes');
    ylim([0, 100]);
end