%% barrier_lyapunov.m — Lyapunov-ellipsoid barrier certificate
%
%  For dx/dt = A_cl x + B_w w with A_cl Hurwitz and ||w||_inf <= w_bar:
%
%    1. Solve A_cl' P + P A_cl = -Qbar  (Qbar > 0, chosen = I).
%       Then V(x) = x' P x is a Lyapunov function for the undisturbed
%       system, with dV/dt = -x'x - x'(Qbar-I)x (here Qbar=I gives -x'x).
%
%    2. Under bounded disturbance:
%         dV/dt = -x'x + 2 x' P B_w w
%               <= -||x||^2 + 2 ||P B_w|| w_bar ||x||.
%       dV/dt <= 0 whenever ||x|| >= 2 ||P B_w|| w_bar.
%       So the ball B_r := {x : ||x|| <= 2 ||P B_w|| w_bar} contains
%       the set where V can still grow.  Any level set {V <= c} that
%       contains B_r is forward-invariant.
%
%    3. Smallest such c:  c* = lambda_max(P) * r^2, where r = 2||P B_w||w_bar.
%
%    4. Safety: the barrier is B(x) = V(x) - gamma, with gamma chosen
%       large enough to contain X_entry but small enough that the level
%       set stays inside X_safe.  We report whether such a gamma exists.
%
%  This is an ellipsoidal over-approximation, generally much looser than
%  the box/zonotope reach in reach_operation.m, but it gives a *certificate*
%  (a closed-form invariant function) rather than just a numerical tube.

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

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

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

Q_lqr = diag([1, 1e-3, 1e-3, 1e-3, 1e-3, 1e-3, 1e-2, 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;

%% ===== Solve Lyapunov equation =====
% A_cl' P + P A_cl + Qbar = 0.  Qbar shaped to weight T_c heavily so the
% resulting ellipsoidal invariant sets are tight in the T_c direction.
% Without shaping, isotropic Qbar = I gives ellipsoids stretched along
% the slow-precursor directions, making the T_c safety bound useless.
Qbar = diag([1, 1e-4, 1e-4, 1e-4, 1e-4, 1e-4, 1e-4, 1, 1e3, 1]);
P = lyap(A_cl.', Qbar);
assert(all(eig(P) > 0), 'P not positive definite');

%% ===== Safety spec (used by sweep and final check) =====
e9 = zeros(10, 1);  e9(9) = 1;
delta_safe_Tc = 5.0;

%% ===== Disturbance bound =====
% |w| <= w_bar  where w = Q_sg - Q_nom.  Take the same 15% down-load as
% reach_operation.m.
w_bar = 0.15 * plant.P0;

% --- Invariant-level computation ---
% dV/dt = -x' Qbar x + 2 x' P B_w w.
% Taking the worst w = w_bar * sign(x' P B_w), the scalar g = x' P B_w:
%   dV/dt <= -x' Qbar x + 2 w_bar |g|.
% Let u = P^{1/2} x (so V = ||u||^2).  Then |g| = |u' P^{-1/2} P B_w|
%   <= ||u|| * ||P^{-1/2} P B_w|| = sqrt(V) * sqrt(B_w' P B_w).
% And x' Qbar x >= lambda_min(P^{-1/2} Qbar P^{-1/2}) * V  (call this mu).
% So dV/dt <= -mu V + 2 w_bar sqrt(B_w' P B_w) sqrt(V).
% dV/dt <= 0 whenever  sqrt(V) >= 2 w_bar sqrt(B_w' P B_w) / mu,
% i.e. V >= (2 w_bar sqrt(B_w' P B_w) / mu)^2  := c_inv.
%
% This is much tighter than the isotropic ball bound — it uses the fact
% that B_w only pokes one direction of the ellipsoid.
P_half     = sqrtm(P);
P_half_inv = inv(P_half);
mu         = min(eig(P_half_inv * Qbar * P_half_inv));
g_bound    = sqrt(B_w.' * P * B_w);      % sqrt(B_w' P B_w)
c_inv      = (2 * w_bar * g_bound / mu)^2;

fprintf('\n=== Lyapunov barrier certificate ===\n');
fprintf('  lambda_min(P)      = %.3e\n', min(eig(P)));
fprintf('  lambda_max(P)      = %.3e\n', max(eig(P)));
fprintf('  sqrt(B_w'' P B_w)  = %.3e\n', g_bound);
fprintf('  mu (Qbar eig on P-metric) = %.3e\n', mu);
fprintf('  w_bar  (15%% P0)    = %.3e W\n', w_bar);
fprintf('  c_inv (invariant level) = %.3e\n', c_inv);

%% ===== Containment of initial set =====
% Initial set: box around x_op with halfwidth delta_entry (matches reach_operation).
delta_entry = [0.01 * x_op(1);
               0.001 * abs(x_op(2:7));
               0.1; 0.1; 0.1];

% Worst-case V over the initial box: max x'Px = sum over all 2^n corners.
% For small n we could enumerate, but the sharper bound is
%   max V(dx) = delta_entry' * |P| * delta_entry
% (elementwise abs of P), which is the L1 energy bound.
c_entry = delta_entry.' * abs(P) * delta_entry;

fprintf('\n  c_entry (bound on V over initial box) = %.3e\n', c_entry);

gamma = max(c_entry, c_inv);   % barrier level must contain both
fprintf('  gamma (barrier level) = %.3e\n', gamma);
if gamma == c_entry
    fprintf('  (initial set drives gamma — invariant piece already inside entry)\n');
else
    fprintf('  (disturbance inflation drives gamma)\n');
end

%% ===== Sweep Qbar(9,9) to find the tightest safe barrier =====
% The isotropic Lyapunov bound is conservative because the "slow decay"
% direction dominates mu even when T_c is tightly controlled.  Sweep the
% T_c weight to find a Qbar that yields a sub-5K barrier if one exists
% for this LQR design.
fprintf('\n=== Sweeping Qbar(T_c) weight ===\n');
weights = [1e1, 1e2, 1e3, 1e4, 1e5, 1e6];
best_dTc = inf;  best_w = NaN;  best_gamma = NaN;  best_P = [];
for wTc = weights
    Qbar_s = Qbar;  Qbar_s(9,9) = wTc;
    try
        Ps = lyap(A_cl.', Qbar_s);
    catch
        continue
    end
    if any(eig(Ps) <= 0), continue, end
    Ph = sqrtm(Ps);  Phi = inv(Ph);
    mu_s = min(eig(Phi * Qbar_s * Phi));
    g_s  = sqrt(B_w.' * Ps * B_w);
    ci_s = (2 * w_bar * g_s / mu_s)^2;
    ce_s = delta_entry.' * abs(Ps) * delta_entry;
    g_s_level = max(ci_s, ce_s);
    Pinv_s = inv(Ps);
    dTc_s = sqrt(g_s_level * (e9.' * Pinv_s * e9));
    fprintf('  Qbar(9,9) = %.0e  ->  gamma = %.3e,  max|dT_c| = %7.3f K\n', ...
            wTc, g_s_level, dTc_s);
    if dTc_s < best_dTc
        best_dTc = dTc_s;  best_w = wTc;  best_gamma = g_s_level;  best_P = Ps;
    end
end
fprintf('  Best: Qbar(9,9) = %.0e  ->  max|dT_c| = %.3f K\n', best_w, best_dTc);
if best_dTc <= delta_safe_Tc
    fprintf('  *** TIGHT BARRIER FOUND: V(x) = x.'' P_best x - gamma ***\n');
    P = best_P;  gamma = best_gamma;
end

%% ===== Safety: does the gamma-level set fit inside X_safe? =====
% X_safe = { x : |T_c - T_c0| <= 5 K },  i.e. |e_9.' * dx| <= 5.
% Max |e_9.' * dx| over {dx : dx' P dx <= gamma} is sqrt(gamma * e_9' P^-1 e_9).
Pinv = inv(P);
max_dTc_on_ellipsoid = sqrt(gamma * (e9.' * Pinv * e9));

fprintf('\n=== Safety check on T_c ===\n');
fprintf('  Max |dT_c| on gamma-ellipsoid = %.3f K\n', max_dTc_on_ellipsoid);
fprintf('  Safe band                     = +/- %.1f K\n', delta_safe_Tc);
if max_dTc_on_ellipsoid <= delta_safe_Tc
    fprintf('  BARRIER VALID: V(x) = x.''Px - %.3e  certifies T_c safety.\n', gamma);
else
    fprintf('  *** BARRIER TOO LOOSE *** - ellipsoid reach into unsafe region.\n');
    fprintf('  Try a tighter LQR (bigger Q_Tc or smaller R) or tighter X_entry.\n');
end

save(fullfile('.', 'barrier_lyapunov_result.mat'), ...
     'P', 'gamma', 'c_entry', 'c_inv', 'w_bar', 'K', 'A_cl', 'delta_entry', ...
     'max_dTc_on_ellipsoid', 'delta_safe_Tc', '-v7');

fprintf('\nSaved barrier to ./barrier_lyapunov_result.mat\n');