close all;
clear all;

% Systemparameter
delta = 5;
omega_0 = 10;

% Simulationsparameter
dt = 0.005;
T = 0:dt:1.5;
steps = length(T);

% Anfangsbedingungen
x_0 = [1; 0]; % Winkel; Drehrate
u_0 = 0; % Sollwinkel für die Steuerung

% Vorberechnungen
omega_0_pow2 = omega_0*omega_0;
statesize = length(x_0);

A = [0, 1; -omega_0_pow2, -2*delta];
B = [0; omega_0_pow2];
Q = diag([1, .1]);
R = 1;
[K, ~, ~] = lqr(A, B, Q, R);

% Speicher für Simulationsergebnisse allozieren und initialisieren
x = zeros(steps, statesize);
x2 = zeros(steps, statesize);
x(1,:) = x_0; % Anfangsbedingung übernehmen
x2(1,:) = x_0; % Anfangsbedingung übernehmen
u = zeros(steps, 1);

% Simulation
for i = 2:steps
  % Gesteuert
  x_dot = [          x(i-1, 2);
            -2*delta*x(i-1, 2) - omega_0_pow2*sin(x(i-1, 1) - u_0)];
  x(i,:) = x(i-1,:) + x_dot'*dt;
  
  % Geregelt
  %A = [0, 1; -omega_0_pow2*cos(x2(i-1,1) - u(i-1)), -2*delta];
  %B = [0; omega_0_pow2*cos(x2(i-1,1) - u(i-1))];
  %[K, ~, ~] = lqr(A, B, Q, R);
  u(i) = -K*x2(i-1,:)';
  x_dot = [          x2(i-1, 2);
            -2*delta*x2(i-1, 2) - omega_0_pow2*sin(x2(i-1, 1) - u(i))];
  x2(i,:) = x2(i-1,:) + x_dot'*dt;
end

% Plotten
plot(T, x, T, x2, T, u);
xlabel("t [s]");
legend('Winkel gesteuert', 'Drehrate gesteuert', 'Winkel geregelt', 'Drehrate geregelt', 'u', 'location', 'southeast');