#!/usr/bin/env julia
#
# reach_operation.jl — operation-mode linear reach in Julia.  **STUB**.
#
# Attempt to reproduce reachability/reach_operation.m using
# ReachabilityAnalysis.jl.
#
# STATUS: does NOT yet produce a useful result.  The closed-loop system
# is strongly stiff (eigvals spanning 0.012 to 65 /s) and has states
# with magnitudes differing by ~10 orders of magnitude (precursors C_i
# ~ 1e5 due to 1/Lambda, temperatures ~ 300).  LGG09 / GLGM06 / BFFPSV18
# all blow up numerically over the 600s horizon, returning envelopes
# ranging from 2757 K to 1e37 K instead of the known-tight 0.03 K.
#
# Likely fix: diagonal state scaling S such that A_cl_scaled = S A_cl S^{-1}
# has O(1) entries, run reach in scaled coordinates, then invert S.
# Also worth trying: ASB07 (adaptive step) or the Taylor-model schemes.
# Left as future work — the hand-rolled MATLAB zonotope in
# reachability/reach_linear.m gives a valid result today, so the Julia
# port is priority-B.

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

using LinearAlgebra
using MatrixEquations
using ReachabilityAnalysis, LazySets

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

plant = pke_params()
x_op  = pke_initial_conditions(plant)

# --- Linearize around the operating point ---
A, B, B_w, _, _, _ = pke_linearize(plant)

# --- LQR gain (same Q, R as the MATLAB side) ---
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)    # 1 x 10
A_cl = A - reshape(B, :, 1) * K

println("\n=== Julia closed-loop spectrum ===")
ev = eigvals(A_cl)
println("  max Re(eig) = ", round(maximum(real.(ev)); sigdigits=4))
println("  min Re(eig) = ", round(minimum(real.(ev)); sigdigits=4))

# --- Reach-set problem: dx/dt = A_cl dx + B_w w, dx(0) ∈ X0, w ∈ W ---
delta_entry = [0.01 * x_op[1];
               0.001 .* abs.(x_op[2:7]);
               0.1; 0.1; 0.1]
X0 = Hyperrectangle(zeros(10), delta_entry)

Q_nom = plant.P0
w_lo, w_hi = -0.15 * plant.P0, 0.0
W  = Interval(w_lo, w_hi)
B_w_col = reshape(B_w, :, 1)

# Encode the bounded disturbance as a bounded input:
#   dx/dt = A_cl x + B_w u,   u ∈ W.
# Direct constructor since the @system macro's dialect is package-version sensitive.
sys = ConstrainedLinearControlContinuousSystem(A_cl, B_w_col, Universe(10), W)
prob = InitialValueProblem(sys, X0)
sol  = solve(prob; T=600.0, alg=GLGM06(; δ=0.5, max_order=20))

# Extract T_c envelope via support function queries.
flow   = flowpipe(sol)
e9     = zeros(10); e9[9] = 1.0
T_c_hi = [ρ(e9,  set(R)) for R in flow]
T_c_lo = [-ρ(-e9, set(R)) for R in flow]

println("\n=== Operation-mode reach envelope on T_c (deviation from T_c0) ===")
println("  min dT_c = ", round(minimum(T_c_lo); digits=4), " K")
println("  max dT_c = ", round(maximum(T_c_hi); digits=4), " K")
println("  safety band |dT_c| <= 5.0 K")
if maximum(abs.([T_c_lo; T_c_hi])) <= 5.0
    println("  OK: Julia reach set inside safety band.")
else
    println("  *** VIOLATION ***")
end

# Save the envelope for later comparison
using MAT
matwrite(joinpath(@__DIR__, "..", "..", "reachability", "julia_reach_operation.mat"),
         Dict("T_c_lo" => T_c_lo, "T_c_hi" => T_c_hi,
              "A_cl" => A_cl, "K" => Matrix(K), "delta_entry" => delta_entry))
println("\nSaved envelope to reachability/julia_reach_operation.mat")