function [t, x, u] = hw4_cvutID()
%% Linearize and discretize the system
Ts = 1/100;

%% Design the regulator

%% Simulation
T0 = 0;
Tf = 3;
t = T0:Ts:Tf;
x = zeros(size(A,1), numel(t));
u = zeros(1, numel(t));

x(:, 1) = [1;0;0;0];

for i=1:numel(t)
    % Compute the control law
    u(i) = 0;
    
    % Simulate the system response for the current control period
    [~,y] = ode45(@(t, y) quadrotor_nonlin(t, y, u(i)), [0 Ts], x(:,i));
    x(:,i+1) = y(end,:)';
end

subplot(211)
plot(t, x(:,1:end-1)')
hold on
plot([T0 Tf], pi/6*[1 1], 'k--')
plot([T0 Tf], -pi/6*[1 1], 'k--')
hold off
grid on

subplot(212)
plot(t, u)
hold on
plot([T0 Tf], 100*[1 1], 'k--')
plot([T0 Tf], -100*[1 1], 'k--')
hold off
grid on

end

function dxdt = quadrotor_nonlin( t, x, alpha )

% x = [y, dy, th, dth]
dxdt = [0;0;0;0];

alpha = max(min(alpha, 100), -100);

dxdt(1) = x(2);
dxdt(2) = -10*tan(x(3));
dxdt(3) = x(4);
dxdt(4) = alpha;

end