%% barrier_compare_OL_CL.m — does LQR help the Lyapunov barrier, or not?
%
%  Head-to-head: solve the same Lyapunov-ellipsoid barrier analysis on
%    (i)  the open-loop plant  A  (u = 0)
%    (ii) the LQR closed loop  A_cl = A - B*K
%  with identical Qbar and identical disturbance bound.  Report per-
%  halfspace bounds against inv2_holds so we can see exactly where LQR
%  helps and where it doesn't.
%
%  The plant is already Hurwitz open-loop (max Re(eig) = -0.012 /s), so
%  the Lyapunov equation has a solution in both cases.

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

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

[A, B, B_w, ~, ~, ~] = pke_linearize(plant, x_op, 0, plant.P0);

%% ===== LQR gain =====
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;

%% ===== Shared pieces =====
Qbar  = diag([1, 1e-4, 1e-4, 1e-4, 1e-4, 1e-4, 1e-4, 1, 1e2, 1]);  % best from the earlier sweep
w_bar = 0.15 * plant.P0;

delta_entry = [0.01 * x_op(1);
               0.001 * abs(x_op(2:7));
               0.1; 0.1; 0.1];

inv2 = pred.mode_invariants.inv2_holds;
A_inv = inv2.A_poly;  b_inv = inv2.b_poly;  comps = inv2.components;
b_dev = b_inv - A_inv * x_op;   % headroom in deviation coords

%% ===== Helper =====
    function [gamma, maxima, eig_cl] = run_case(Acase, B_w, Qbar, w_bar, delta_entry, A_inv, b_dev)
        % Solve Lyapunov, compute invariant level, per-halfspace maxima.
        P = lyap(Acase.', Qbar);
        Ph = sqrtm(P);  Phi = inv(Ph);
        mu = min(eig(Phi * Qbar * Phi));
        g_bound = sqrt(B_w.' * P * B_w);
        c_inv = (2 * w_bar * g_bound / mu)^2;
        c_entry = delta_entry.' * abs(P) * delta_entry;
        gamma = max(c_inv, c_entry);
        Pinv = inv(P);
        maxima = zeros(size(A_inv, 1), 1);
        for k = 1:size(A_inv, 1)
            a = A_inv(k, :).';
            maxima(k) = sqrt(gamma * (a.' * Pinv * a));
        end
        eig_cl = eig(Acase);
    end

%% ===== Run both =====
[gamma_OL, max_OL, eig_OL] = run_case(A,    B_w, Qbar, w_bar, delta_entry, A_inv, b_dev);
[gamma_CL, max_CL, eig_CL] = run_case(A_cl, B_w, Qbar, w_bar, delta_entry, A_inv, b_dev);

%% ===== Report =====
fprintf('\n=== Open-loop vs LQR closed-loop Lyapunov barrier ===\n');
fprintf('  max Re(eig)     OL = %.3e   CL = %.3e\n', max(real(eig_OL)), max(real(eig_CL)));
fprintf('  min Re(eig)     OL = %.3e   CL = %.3e\n', min(real(eig_OL)), min(real(eig_CL)));
fprintf('  gamma           OL = %.3e   CL = %.3e   (ratio CL/OL = %.3g)\n', ...
        gamma_OL, gamma_CL, gamma_CL/gamma_OL);

fprintf('\n  Per-halfspace max |a'' dx| on gamma-ellipsoid:\n');
fprintf('  %-22s  %-10s  %-12s  %-12s  %-12s  %-10s\n', ...
        'halfspace', 'headroom', 'open-loop', 'closed-loop', 'CL - OL', 'ratio');
for k = 1:size(A_inv, 1)
    ratio = max_CL(k) / max_OL(k);
    fprintf('  %-22s  %10.3f  %12.3f  %12.3f  %+12.3f  %8.3fx\n', ...
            comps{k}, b_dev(k), max_OL(k), max_CL(k), max_CL(k) - max_OL(k), ratio);
end

fprintf('\n  Interpretation:\n');
fprintf('    - CL < OL on a row ==> LQR tightens that halfspace.\n');
fprintf('    - CL ~= OL       ==> LQR does not help that direction at all.\n');
fprintf('    - CL > OL        ==> LQR made that direction WORSE for the barrier (!!).\n');