%% reach_operation.m — linear reach set for operation mode (LQR closed-loop)
%
%  *** SOUNDNESS STATUS: APPROXIMATE, NOT SOUND. ***
%
%  This file computes a reach tube for the *linearized* closed-loop
%  system (A_cl = A - BK around x_op) under bounded Q_sg.  The tube
%  itself is a sound over-approximation of the LINEAR model's reach
%  set — it uses conservative box hulls and elementwise-absolute-value
%  matrix propagation.  But the LINEAR MODEL is only an approximation
%  of the real nonlinear plant (pke_th_rhs.m), so the result is not a
%  sound reach tube for the actual plant.
%
%  To upgrade to a sound result, pick one:
%    (a) Nonlinear reach directly (CORA's nonlinearSys, JuliaReach's
%        BlackBoxContinuousSystem).  Expensive, honest.
%    (b) Linear reach + Taylor-remainder inflation: compute an upper
%        bound on ||f_nonlinear(x,u) - (A x + B u)|| over the reach
%        set and inflate the linear tube by that bound.  Cheaper,
%        requires a Hessian-norm estimate.
%  Tracked as a thesis-blocking todo.  For now, the 5-orders-of-margin
%  buffer (|dT_c| ~ 0.03 K vs safety band 5 K) gives us a lot of room
%  to absorb linearization error, but that's not a proof.
%
%  Compute a reach-tube over-approximation starting from a box around
%  x_op, under LQR feedback, with Q_sg in a specified interval.  Check
%  that T_avg stays inside the t_avg_in_range predicate for all t in
%  [0, T_final].
%
%  This is the *continuous-mode obligation* for q_operation:
%     X_entry  := { x : |x - x_op| <= delta_entry }
%     W        := [Q_min, Q_max]
%     X_safe   := { x : |T_c - T_c0| <= delta_safe }
%     Obligation: ReachTube(X_entry, W, [0, T_final]) subset X_safe.
%
%  If this passes, we've discharged the Thrust-3 verification for one
%  continuous mode at the level the thesis calls for.

clear; clc; close all;
addpath('../plant-model', '../plant-model/controllers');

plant = pke_params();
x_op  = pke_initial_conditions(plant);

%% ===== Closed-loop linearization =====
[A, B, B_w, ~, ~, ~] = pke_linearize(plant, x_op, 0, plant.P0);

% LQR gain using the same Q, R as ctrl_operation_lqr.m.  Keep these in
% sync if you retune there.
Q_lqr = diag([1, 1e-3, 1e-3, 1e-3, 1e-3, 1e-3, 1e-3, 1e-2, 1e2, 1]);
R_lqr = 1e6;
try
    K = lqr(A, B, Q_lqr, R_lqr);
catch
    [~, ~, K] = icare(A, B, Q_lqr, R_lqr);
end

A_cl = A - B*K;

fprintf('\n=== Closed-loop spectrum (A - BK) ===\n');
eigs_cl = eig(A_cl);
fprintf('  max Re(eig) = %.3e\n', max(real(eigs_cl)));
fprintf('  min Re(eig) = %.3e\n', min(real(eigs_cl)));
assert(all(real(eigs_cl) < -1e-8), 'A_cl not Hurwitz — gain tuning issue');

%% ===== Define sets =====
% X_entry: 1% box on n, 0.1% boxes on precursors (their magnitudes are
% huge due to 1/Lambda), 0.1 K on fuel, 0.1 K on coolant, 0.1 K on cold leg.
% This represents "we entered operation mode near the steady state".
delta_entry = [0.01 * x_op(1);                  % n
               0.001 * abs(x_op(2:7));          % C1..C6
               0.1;                             % T_f [C]
               0.1;                             % T_c [C]
               0.1];                            % T_cold [C]

% Disturbance: Q_sg = [0.85*P0, 1.00*P0] -> this captures up to a 15%
% down-step load demand (realistic load-follow envelope).
Q_nom = plant.P0;
Q_min = 0.85 * plant.P0;
Q_max = 1.00 * plant.P0;
dQ_lo = Q_min - Q_nom;     % -0.15 * P0
dQ_hi = Q_max - Q_nom;     % 0

% X_safe: |T_c - T_c0| <= 5 K (roughly matching the "t_avg_in_range"
% predicate window — adjust once the FRET predicate threshold is locked).
delta_safe_Tc = 5.0;   % [C]

%% ===== Reach set =====
% Propagate in deviation coordinates: dx = x - x_op.
tspan = [0, 600];
dt    = 0.5;

[T, R_lo, R_hi, C] = reach_linear(A_cl, B_w, zeros(10,1), delta_entry, dQ_lo, dQ_hi, tspan, dt);

% Translate back to absolute coordinates for reporting
Xabs_lo = R_lo + x_op;
Xabs_hi = R_hi + x_op;
Cabs    = C    + x_op;

%% ===== Safety check =====
T_c_lo = Xabs_lo(9, :);
T_c_hi = Xabs_hi(9, :);

violation_mask = (T_c_hi > plant.T_c0 + delta_safe_Tc) | ...
                 (T_c_lo < plant.T_c0 - delta_safe_Tc);
fprintf('\n=== Operation-mode reach-set safety ===\n');
fprintf('  Horizon               = [%g, %g] s\n', tspan(1), tspan(2));
fprintf('  Entry box T_c  [C]   = [%.3f, %.3f]  (x_op +/- %.1f C)\n', ...
        x_op(9) - delta_entry(9), x_op(9) + delta_entry(9), delta_entry(9));
fprintf('  Disturbance Q_sg     = [%.3f, %.3f] MW\n', Q_min/1e6, Q_max/1e6);
fprintf('  Safe band on T_c     = x_op(T_c0) +/- %.1f C  -> [%.3f, %.3f]\n', ...
        delta_safe_Tc, plant.T_c0 - delta_safe_Tc, plant.T_c0 + delta_safe_Tc);
fprintf('  Reach T_c envelope   = [%.3f, %.3f]\n', min(T_c_lo), max(T_c_hi));
if any(violation_mask)
    t_first = T(find(violation_mask, 1));
    fprintf('  *** SAFETY VIOLATED at t = %.2f s ***\n', t_first);
else
    fprintf('  OK: reach set stays inside the safe band.\n');
end

%% Per-state reach-set growth diagnostic (final time vs initial)
state_names = {'n','C1','C2','C3','C4','C5','C6','T_f','T_c','T_cold'};
fprintf('\n=== Reach-set width at t=0 vs t=T_final ===\n');
fprintf('  %-7s  %-14s  %-14s  %-8s\n', 'state', 'init halfwidth', 'final halfwidth', 'ratio');
for i = 1:10
    hi = 0.5 * (R_hi(i, 1)   - R_lo(i, 1));
    hf = 0.5 * (R_hi(i, end) - R_lo(i, end));
    fprintf('  %-7s  %-14.4e  %-14.4e  %-8.2f\n', state_names{i}, hi, hf, hf/max(hi,eps));
end

%% ===== Plots =====
figdir = fullfile('..', 'docs', 'figures');
if ~exist(figdir, 'dir'), mkdir(figdir); end
CtoF = @(T) T*9/5 + 32;

% Two-panel plot: wide view with safety band, zoom view showing actual tube.
figure('Position', [100 80 1400 500], 'Name', 'Reach tube: T_c');

subplot(1,2,1);
fill([T; flipud(T)], CtoF([T_c_hi.'; flipud(T_c_lo.')]), [1.0 0.85 0.85], ...
     'EdgeColor', 'none'); hold on;
plot(T, CtoF(Cabs(9, :)), 'r-',  'LineWidth', 1.2);
yline(CtoF(plant.T_c0 + delta_safe_Tc), 'k--', 'LineWidth', 1.0);
yline(CtoF(plant.T_c0 - delta_safe_Tc), 'k--', 'LineWidth', 1.0);
yline(CtoF(plant.T_c0),                 'k:',  'LineWidth', 1.0);
xlabel('Time [s]'); ylabel('T_{avg} [F]'); grid on;
title('Safety-band view');
legend('reach tube', 'nominal', 'safety +/- 5 C', 'Location', 'best');

subplot(1,2,2);
Tc_dev_lo = T_c_lo.' - plant.T_c0;    % M x 1, deviation in K
Tc_dev_hi = T_c_hi.' - plant.T_c0;
fill([T; flipud(T)], [Tc_dev_hi; flipud(Tc_dev_lo)], [1.0 0.85 0.85], ...
     'EdgeColor', 'none'); hold on;
plot(T, Cabs(9, :).' - plant.T_c0, 'r-', 'LineWidth', 1.2);
yline(0, 'k:', 'LineWidth', 1.0);
xlabel('Time [s]'); ylabel('T_{avg} - T_{c0} [K]'); grid on;
max_dev = max(abs([Tc_dev_lo; Tc_dev_hi]));
title(sprintf('Zoomed: max |dT_c| = %.3e K', max_dev));

sgtitle(sprintf('Operation-mode reach tube, LQR, Q_{sg} in [%.0f%%, %.0f%%] P_0', ...
                100*Q_min/Q_nom, 100*Q_max/Q_nom));
exportgraphics(gcf, fullfile(figdir, 'reach_operation_Tc.png'), 'Resolution', 150);

figure('Position', [100 80 1100 500], 'Name', 'Reach tube: n');
fill([T; flipud(T)], [R_hi(1,:).' + x_op(1); flipud(R_lo(1,:).' + x_op(1))], ...
     [0.85 0.85 1.0], 'EdgeColor', 'none'); hold on;
plot(T, Cabs(1, :), 'b-', 'LineWidth', 1.2);
xlabel('Time [s]'); ylabel('n'); grid on;
title('Operation mode reach tube on normalized power');
legend('reach tube', 'nominal', 'Location', 'best');
exportgraphics(gcf, fullfile(figdir, 'reach_operation_n.png'), 'Resolution', 150);

save(fullfile('.', 'reach_operation_result.mat'), ...
     'T', 'R_lo', 'R_hi', 'C', 'Xabs_lo', 'Xabs_hi', 'Cabs', ...
     'K', 'A_cl', 'x_op', 'delta_entry', 'Q_min', 'Q_max', 'delta_safe_Tc', '-v7');

fprintf('\nSaved reach result to ./reach_operation_result.mat\n');