#!/usr/bin/env julia
#
# sim_heatup.jl — nominal heatup trajectory sim in Julia.
#
# Closed-loop nonlinear simulation using the SAME RHS form as
# reach_heatup_nonlinear.jl (augmented time x[11], inlined constants)
# but via OrdinaryDiffEq, not TMJets.  Purpose: verify the @taylorize-able
# RHS matches the MATLAB heatup baseline.  Also generates a nominal
# trajectory that reach tubes can be checked against.

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

using OrdinaryDiffEq

# Constants (match reach_heatup_nonlinear.jl).
const LAMBDA  = 1e-4
const BETA_1, BETA_2, BETA_3, BETA_4, BETA_5, BETA_6 =
    0.000215, 0.001424, 0.001274, 0.002568, 0.000748, 0.000273
const BETA    = BETA_1 + BETA_2 + BETA_3 + BETA_4 + BETA_5 + BETA_6
const LAM_1, LAM_2, LAM_3, LAM_4, LAM_5, LAM_6 =
    0.0124, 0.0305, 0.111, 0.301, 1.14, 3.01

const P0   = 1e9
const M_F, C_F, M_C, C_C, HA, W_M, M_SG =
    50000.0, 300.0, 20000.0, 5450.0, 5e7, 5000.0, 30000.0
const ALPHA_F, ALPHA_C = -2.5e-5, -1e-4
const T_COLD0 = 290.0
const DT_CORE = P0 / (W_M * C_C)
const T_HOT0  = T_COLD0 + DT_CORE
const T_C0    = (T_HOT0 + T_COLD0) / 2
const T_F0    = T_C0 + P0 / HA

const T_STANDBY    = T_C0 - 33.333333
const RAMP_RATE_CS = 28.0 / 3600
const KP_HEATUP    = 1e-4

function rhs_heatup_sim!(dx, x, p, t)
    rho = KP_HEATUP * (T_STANDBY + RAMP_RATE_CS * x[11] - x[9])
    dx[1] = (rho - BETA) / LAMBDA * x[1] +
            LAM_1*x[2] + LAM_2*x[3] + LAM_3*x[4] +
            LAM_4*x[5] + LAM_5*x[6] + LAM_6*x[7]
    dx[2] = (BETA_1 / LAMBDA) * x[1] - LAM_1 * x[2]
    dx[3] = (BETA_2 / LAMBDA) * x[1] - LAM_2 * x[3]
    dx[4] = (BETA_3 / LAMBDA) * x[1] - LAM_3 * x[4]
    dx[5] = (BETA_4 / LAMBDA) * x[1] - LAM_4 * x[5]
    dx[6] = (BETA_5 / LAMBDA) * x[1] - LAM_5 * x[6]
    dx[7] = (BETA_6 / LAMBDA) * x[1] - LAM_6 * x[7]
    dx[8]  = (P0 * x[1] - HA * (x[8] - x[9])) / (M_F * C_F)
    dx[9]  = (HA * (x[8] - x[9]) - 2 * W_M * C_C * (x[9] - x[10])) / (M_C * C_C)
    dx[10] = (2 * W_M * C_C * (x[9] - x[10])) / (M_SG * C_C)
    dx[11] = 1.0
    return nothing
end

# IC = midpoint of X_entry polytope.
n0 = 0.001
C0 = [BETA_1/(LAM_1*LAMBDA), BETA_2/(LAM_2*LAMBDA), BETA_3/(LAM_3*LAMBDA),
      BETA_4/(LAM_4*LAMBDA), BETA_5/(LAM_5*LAMBDA), BETA_6/(LAM_6*LAMBDA)] .* n0
x0 = [n0; C0; T_STANDBY + 5; T_STANDBY + 6; T_STANDBY + 4; 0.0]

tspan = (0.0, 300.0)

# Rodas5 is stiff-aware.
prob = ODEProblem(rhs_heatup_sim!, x0, tspan)
sol  = solve(prob, Rodas5(); reltol=1e-8, abstol=1e-10)

println("\n=== Heatup nominal trajectory (saturation disabled, Julia) ===")
println("  t=0       : n=$(round(sol.u[1][1]; sigdigits=3))  T_c=$(round(sol.u[1][9]; digits=2)) C")
for t_check in (10.0, 60.0, 120.0, 300.0)
    u = sol(t_check)
    println("  t=$(lpad(Int(t_check), 4))s    : n=$(round(u[1]; sigdigits=3))  T_c=$(round(u[9]; digits=2)) C  T_f=$(round(u[8]; digits=2)) C")
end