clear; clc; close all;

%% PART 1: SIMPLE SYSTEM CONTROL (NO SETPOINT, NO EXOGENOUS VARIABLES)
disp('--- PART 1: SIMPLE SYSTEM CONTROL ---');

% Define system matrices
A = [0 1; -2 -3];  % State matrix
B = [0; 1];        % Input matrix
Q = 10*eye(2);        % State weighting matrix
R = 100;             % Control effort weighting scalar

% Solve ARE for state-feedback gain
[P, ~, ~] = care(A, B, Q, R);
K = R \ (B' * P);

% Closed-loop system matrix
A_cl = A - B * K;

% Initial state
x0 = [1; 1];  % Start at some arbitrary initial state

% Simulate the system
dt = 0.01;     % Time step
T = 10;        % Simulation duration
time = 0:dt:T; % Time vector
x = x0;        % Initialize state
X = zeros(length(x0), length(time)); % Store states over time

for i = 1:length(time)
    X(:, i) = x;            % Store current state
    u = -K * x;             % Control law
    x = x + (A_cl * x) * dt; % Update state using closed-loop dynamics
end

% Plot results
figure;
subplot(2, 1, 1);
plot(time, X(1, :), 'b', 'LineWidth', 1.5); hold on;
plot(time, X(2, :), 'r', 'LineWidth', 1.5);
xlabel('Time (s)'); ylabel('State Variables');
legend('x_1 (Position)', 'x_2 (Velocity)');
title('State Evolution in the Simple System');

subplot(2, 1, 2);
plot(time, -K * X, 'm', 'LineWidth', 1.5);
xlabel('Time (s)'); ylabel('Control Input');
title('Control Input Over Time');

%% PART 2: SETPOINT REGULATION
disp('--- PART 2: SETPOINT REGULATION ---');

% Define system matrices (reusing from PART 1)
x_ref = [2; 0]; % Desired setpoint (e.g., position = 2, velocity = 0)

% Modify dynamics to include setpoint regulation
u_ss = -inv(B' * B) * (B' * A * x_ref); % Steady-state control
Q = 0.01*eye(2); % Reuse weighting matrices
R = 100;

% Solve ARE again for feedback gain
[P, ~, ~] = care(A, B, Q, R);
K = R \ (B' * P);

% Simulate the system
x = x0; % Reset initial condition
X_setpoint = zeros(length(x0), length(time)); % Store states over time
U_control = zeros(1, length(time)); % Store control inputs

for i = 1:length(time)
    X_setpoint(:, i) = x;                % Store current state
    e = x - x_ref;                       % Compute error
    u = u_ss - K * e;                    % Control law with setpoint regulation
    U_control(i) = u;                    % Store control input
    x = x + (A * x + B * u) * dt;        % Update state
end

% Plot results
figure;
subplot(3, 1, 1);
plot(time, X_setpoint(1, :), 'b', 'LineWidth', 1.5); hold on;
plot(time, x_ref(1) * ones(size(time)), 'r--', 'LineWidth', 1.5);
xlabel('Time (s)'); ylabel('x_1 (Position)');
legend('x_1 (Actual)', 'x_1 (Reference)');
title('Position Tracking with Setpoint Regulation');

subplot(3, 1, 2);
plot(time, X_setpoint(2, :), 'g', 'LineWidth', 1.5); hold on;
plot(time, x_ref(2) * ones(size(time)), 'r--', 'LineWidth', 1.5);
xlabel('Time (s)'); ylabel('x_2 (Velocity)');
legend('x_2 (Actual)', 'x_2 (Reference)');
title('Velocity Tracking with Setpoint Regulation');

subplot(3, 1, 3);
plot(time, U_control, 'm', 'LineWidth', 1.5);
xlabel('Time (s)'); ylabel('Control Input');
title('Control Input Over Time with Setpoint Regulation');
