#!/usr/bin/env julia
#
# barrier_lyapunov.jl — Lyapunov-ellipsoid barrier cert.
# Julia port of reachability/barrier_lyapunov.m.
#
# Sweeps Qbar(T_c) weight to find the tightest quadratic barrier;
# reports per-halfspace margins against inv2_holds.
#
# Expected result: every halfspace exceeded by orders of magnitude.
# This is the structural anisotropy limitation, not a code bug.

using Pkg
Pkg.activate(joinpath(@__DIR__, "..", ".."))

using Printf
using LinearAlgebra
using MatrixEquations
using JSON

include(joinpath(@__DIR__, "..", "..", "src", "pke_params.jl"))
include(joinpath(@__DIR__, "..", "..", "src", "pke_th_rhs.jl"))
include(joinpath(@__DIR__, "..", "..", "src", "pke_linearize.jl"))
include(joinpath(@__DIR__, "..", "..", "src", "load_predicates.jl"))

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

# --- Closed-loop ---
A, B, B_w, _, _, _ = pke_linearize(plant)
Q_lqr = Diagonal([1.0, 1e-3, 1e-3, 1e-3, 1e-3, 1e-3, 1e-3, 1e-2, 1e2, 1.0])
R_lqr = 1e6 * ones(1, 1)
X_ric, _, _ = arec(A, reshape(B, :, 1), R_lqr, Matrix(Q_lqr))
K    = (R_lqr \ reshape(B, 1, :)) * X_ric
A_cl = A - reshape(B, :, 1) * K

# --- Lyapunov ---
# lyapc solves A' P + P A + Q = 0 for P.  Julia's MatrixEquations uses
# lyapc(A', Q) to solve A' P + P A = -Q (note: sign conventions vary).
# Check: for a Hurwitz A, lyapc(A, Q) with Q > 0 returns P > 0.
Qbar = Diagonal([1.0, 1e-4, 1e-4, 1e-4, 1e-4, 1e-4, 1e-4, 1.0, 1e2, 1.0])
P    = lyapc(Matrix(A_cl)', Matrix(Qbar))
@assert all(eigvals(P) .> 0) "P not positive definite"

# --- Disturbance bound ---
w_bar = 0.15 * plant.P0

# Invariant level via the derivation in the MATLAB file.
P_half     = Hermitian(sqrt(Hermitian(P)))
P_half_inv = inv(P_half)
mu         = minimum(eigvals(Symmetric(P_half_inv * Matrix(Qbar) * P_half_inv)))
g_bound    = sqrt(B_w' * P * B_w)
c_inv      = (2 * w_bar * g_bound / mu)^2

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

gamma = max(c_inv, c_entry)

println("\n=== Lyapunov barrier certificate ===")
@printf "  lambda_min(P)            = %.3e\n"  minimum(eigvals(P))
@printf "  lambda_max(P)            = %.3e\n"  maximum(eigvals(P))
@printf "  sqrt(B_w' P B_w)         = %.3e\n"  g_bound
@printf "  mu (Qbar eig on P-metric)= %.3e\n"  mu
@printf "  w_bar  (15%% P0)          = %.3e W\n"  w_bar
@printf "  c_inv  (invariant level) = %.3e\n"  c_inv
@printf "  c_entry (from X_entry)   = %.3e\n"  c_entry
@printf "  gamma                    = %.3e\n"  gamma

# --- inv2_holds per-halfspace barrier check ---
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

Pinv = inv(P)
println("\n=== Lyapunov barrier vs inv2_holds halfspaces ===")
for k in 1:size(A_inv, 1)
    a = A_inv[k, :]
    max_adx = sqrt(gamma * (a' * Pinv * a))
    margin  = b_dev[k] - max_adx
    status  = margin < 0 ? "*** TOO LOOSE ***" : "OK"
    @printf "  [%-22s]  headroom = %8.3f  |  max a'dx = %8.3f  |  margin = %+8.3f  %s\n" comps[k] b_dev[k] max_adx margin status
end

# --- Qbar(T_c) sweep for tightness ---
println("\n=== Qbar(T_c) weight sweep ===")
e9 = zeros(10); e9[9] = 1.0
weights = [1e1, 1e2, 1e3, 1e4, 1e5, 1e6]
let best_dTc = Inf, best_w = NaN, best_gamma = NaN, best_P = nothing
    for wTc in weights
        Qbs = copy(Matrix(Qbar))
        Qbs[9, 9] = wTc
        Ps = try
            lyapc(Matrix(A_cl)', Qbs)
        catch
            continue
        end
        any(eigvals(Ps) .<= 0) && continue
        Phs = Hermitian(sqrt(Hermitian(Ps)))
        Phi = inv(Phs)
        mu_s = minimum(eigvals(Symmetric(Phi * Qbs * 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
        gs   = max(ci_s, ce_s)
        Pinv_s = inv(Ps)
        dTc  = sqrt(gs * (e9' * Pinv_s * e9))
        @printf "  Qbar(9,9) = %.0e  ->  gamma = %.3e,  max|dT_c| = %7.3f K\n" wTc gs dTc
        if dTc < best_dTc
            best_dTc = dTc; best_w = wTc; best_gamma = gs; best_P = Ps
        end
    end
    @printf "  Best: Qbar(9,9) = %.0e  ->  max|dT_c| = %.3f K\n" best_w best_dTc
end