% HVAC Temperature Regulation: Observer + Controller Simulation

% System Matrices
A = [0 1; -2 -1];             % State matrix
B = [0; 0.5];                 % Input matrix
C = [1 0];                    % Output matrix

% Desired Poles
controller_poles = [-20, -30];  % Controller poles for A-BK
observer_poles = [-10, -12];    % Observer poles for A-LC

% Compute Gains
K = place(A, B, controller_poles);    % State-feedback gain
L = place(A', C', observer_poles)';   % Observer gain (transpose for correct dimension)

% Compute Feedforward Gain K_r
K_r = -inv(C * inv(A - B * K) * B);    % Feedforward gain

% Debug Outputs
disp('Feedback Gain K:');
disp(K);
disp('Feedforward Gain K_r:');
disp(K_r);
disp('Eigenvalues of A - BK (Closed-Loop System):');
disp(eig(A - B * K));

% Initial Conditions
x0 = [18; 0];               % Initial actual state [Temperature; Rate]
x_hat0 = [0; 0];            % Initial estimated state

% Simulation Parameters
T_ref = 22;                 % Reference temperature
Tspan = 0:0.01:10;          % Simulation time (0 to 10 seconds)
u_func = @(t) 0;            % External disturbance (can be adjusted)

% Dynamics with Observer and Controller
A_CL = [A - B*K, B*K; zeros(size(A)), A - L*C]; % Combined system
B_CL = [B*K_r; zeros(size(B))];                % Input matrix for combined system
C_CL = [C, zeros(size(C))];                    % Output matrix for combined system
IC = [x0; x_hat0];                             % Initial conditions

% Simulate the system
[t, X] = ode45(@(t, x) combined_dynamics(t, x, A_CL, B_CL, C_CL, T_ref, u_func), Tspan, IC);

% Extract states
x_actual = X(:, 1:2);     % Actual states
x_estimated = X(:, 3:4);  % Estimated states
u_control = -K * x_actual' + K_r * T_ref;  % Control input

% Plot Results
figure;
subplot(3, 1, 1);
plot(t, x_actual(:, 1), 'b', 'LineWidth', 1.5); hold on;
plot(t, T_ref * ones(size(t)), 'r--', 'LineWidth', 1.5);
xlabel('Time (s)');
ylabel('Temperature (^oC)');
legend('T_{indoor}', 'T_{ref}');
title('Temperature Tracking');

subplot(3, 1, 2);
plot(t, x_actual(:, 2), 'g', 'LineWidth', 1.5); hold on;
plot(t, x_estimated(:, 2), 'k--', 'LineWidth', 1.5);
xlabel('Time (s)');
ylabel('Rate of Change');
legend('\dot{T}_{actual}', '\dot{T}_{estimated}');
title('Rate of Temperature Change');

subplot(3, 1, 3);
plot(t, u_control, 'm', 'LineWidth', 1.5);
xlabel('Time (s)');
ylabel('Control Input (u)');
title('Control Effort');

% Local function must appear at the end of the script
function dx = combined_dynamics(t, x, A_CL, B_CL, C_CL, T_ref, u_func)
    u_disturbance = u_func(t);        % External disturbance
    r = T_ref;                       % Reference temperature
    dx = A_CL * x + B_CL * (r + u_disturbance); % Combined dynamics
end
