clear; 

%% Tank Inflow
A = 0.2;
a = 0.01;
g = 9.81;

h_dot = @(t, h) (-a/A)*sqrt(2*g*h);

h0 = 0.2;

tspan = 0:0.001:5;

[t, h] = ode45(h_dot, tspan , h0);

figure(1); 
plot(t, real(h), '-');
xlabel('time t'); ylabel('height h');
grid on;

%% Inverted Pendulum 

c = 0.2;

x_dot = @(t, x) [x(2); sin(x(1)) - c*x(2)];

tspan = [0, 100];

x0s = [-5, 2;
        5, -2;
       0.1, 0.1; 
       -0.1, -0.1];

% Plot state trajecotries 
figure(2); 
hold on; grid on;

for j=1:size(x0s, 1)
    x0s(j, :)
    [t, x] = ode45(x_dot, tspan , x0s(j, :));
    plot(x(:, 1), x(:, 2), '-');
end


% Plot the flow field
[x1s,x2s] = meshgrid(-5:0.2:5,-2:0.2:2);

u = x1s;
v = x2s;

for j=1:size(x1s,1)
    for k=1:size(x1s, 2)
       derivative = x_dot(0, [x1s(j,k), x2s(j,k)]);
       u(j,k) = derivative(1);
       v(j,k) = derivative(2);
    end
end

quiver(x1s,x2s,u,v);
    

%% Continuous Time System

A = [[-0.25, 0.25, 0]; 
     [    0, -0.2, 0.4];
     [ 0, 0, -0.1]];
B = [0; 0; 2];

C = [1 0 0];
D = 0;

sys = ss(A, B, C, D);

figure(3); 

subplot(2,1,1);
step(sys);

subplot(2,1,2);
impulse(sys);

%% Discrete time system

A = [2 0; 
     0 2];
 
B = [1; 1];

C = [1 1];
D = 0;

dt = 1;

sys_cont = ss(A, B, C, []);

sys_dis = c2d(sys_cont,1);

% simulate sys_dis with u_k = [1, 1, 1], x0 = [0,0]
x0 = [0 0]';
U = [1; 1; 1];
[y, t, x] = lsim(sys_dis, U, [], x0);

disp(x);

% compare results
A_dis = exp(2).*eye(2);
B_dis = (-0.5 + 0.5*exp(2)).*[1; 1];

x1 = A_dis*x0 + B_dis*U(1);
x2 = A_dis*x1 + B_dis*U(2);

disp([x0'; x1'; x2']);

lsim(sys_dis, U, [], x0)

