#!/usr/bin/env julia
#
# barrier_compare_OL_CL.jl — head-to-head Lyapunov barrier bounds with
# and without LQR feedback.  Julia port of reachability/barrier_compare_OL_CL.m.
#
# Expected: LQR improves every bound by ~20,000x, but bounds stay
# physically meaningless.  Point is to show the ceiling is plant
# anisotropy, not controller tuning.

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)

A, B, B_w, _, _, _ = pke_linearize(plant)

# LQR gain (same weights as ctrl_operation_lqr).
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

# Shared Qbar — best from the earlier sweep.
Qbar  = Diagonal([1.0, 1e-4, 1e-4, 1e-4, 1e-4, 1e-4, 1e-4, 1.0, 1e2, 1.0])
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

function run_case(Acase)
    P = lyapc(Matrix(Acase)', Matrix(Qbar))
    Ph = Hermitian(sqrt(Hermitian(P)))
    Phi = inv(Ph)
    mu  = minimum(eigvals(Symmetric(Phi * Matrix(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 = [sqrt(gamma * (A_inv[k, :]' * Pinv * A_inv[k, :])) for k in 1:size(A_inv, 1)]
    return gamma, maxima, eigvals(Acase)
end

gamma_OL, max_OL, eig_OL = run_case(A)
gamma_CL, max_CL, eig_CL = run_case(A_cl)

println("\n=== Open-loop vs LQR closed-loop Lyapunov barrier ===")
@printf "  max Re(eig)     OL = %.3e   CL = %.3e\n"  maximum(real.(eig_OL)) maximum(real.(eig_CL))
@printf "  min Re(eig)     OL = %.3e   CL = %.3e\n"  minimum(real.(eig_OL)) minimum(real.(eig_CL))
@printf "  gamma           OL = %.3e   CL = %.3e   (ratio CL/OL = %.3g)\n"  gamma_OL gamma_CL gamma_CL/gamma_OL

println("\n  Per-halfspace max |a' dx| on gamma-ellipsoid:")
@printf "  %-22s  %-10s  %-14s  %-14s  %-14s  %-10s\n"  "halfspace" "headroom" "open-loop" "closed-loop" "CL - OL" "ratio"
for k in 1:size(A_inv, 1)
    ratio = max_CL[k] / max_OL[k]
    @printf "  %-22s  %10.3f  %14.3f  %14.3f  %+14.3f  %10.3gx\n"  comps[k] b_dev[k] max_OL[k] max_CL[k] (max_CL[k] - max_OL[k]) ratio
end

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