##################
# controller
##############

using DelimitedFiles

include("params.jl")

# Struct for Heater-Cooler model
mutable struct HeaterCooler
    T_set::Float64
    ṁ::Float64  # mass flow of oil [kg/s]
    x1::Float64  # Energy in first stage [J]
    x2::Float64  # Energy in second stage [J]
    t0_real::Float64  # Initial real time
    t_real_prev::Float64  # Previous real time
    count::Int64  # Timer count

    function HeaterCooler(params::Params)
        T_set = 0.0  # params.T_cold
        # oletus, virtausnopes on vakio
        ṁ = π * params.R_pipe^2 * params.v * params.rho_oil
        x1 = 0.0 # params.T_initial
        x2 = 0.0

        # Initialize timer fields
        t0_real = time()
        t_real_prev = 0
        count = 0
        new(T_set, ṁ, x1, x2, t0_real, t_real_prev, count)
    end
end

#=
For your system's step response: 
- Rise time ∝ τ/s (inversely proportional to ωₙ) 
- Settling time ∝ τ/(ζs) for underdamped systems
- Oscillation frequency ∝ s/τ when underdamped
=#

function format_time(seconds::Float64, label="elapsed time")
    hours = floor(Int, seconds / 3600)
    remainder = seconds % 3600
    minutes = floor(Int, remainder / 60)
    secs = remainder % 60
    return "$label $(lpad(hours, 2, '0')):$(lpad(minutes, 2, '0')):$(@sprintf("%05.2f", secs))"
end

# Timer function (translated from Python)
function timer(hc::HeaterCooler, t_sim::Float64, dt::Float64, params::Params)
    # print progress every n_steps time step
    n_step = 50
    if hc.count < 4
        t_real = time() - hc.t0_real
        hc.t_real_prev = t_real
    elseif hc.count % n_step == 0
        t_real = time() - hc.t0_real
        t_real_dt = t_real - hc.t_real_prev
        hc.t_real_prev = t_real
        t_real_left = t_real_dt / (n_step * dt) * (params.t_end - t_sim)
        s1 = format_time(t_real, "real time")
        s2 = format_time(t_sim, "simulation time")
        s2b = format_time(params.t_end, " /")
        s3 = format_time(t_real_left, "real time to finish")
        println(s1, " || ", s2, s2b, " || ", s3, " || dt:", sci3e(dt))
    end
    hc.count += 1

end 

function control(hc::HeaterCooler, T_out::Float64, t::Float64, dt::Float64, params::Params)
    function heat_cool(params::Params, hc::HeaterCooler, u::Float64, dt::Float64)
        tf = params.tau_heater
        s = 1.0
        sigma = 1.0
        wn = s / tf
        # u = P / (hc.ṁ * params.cp_oil) + T_out
        dx1 = hc.x2
        dx2 = -wn^2 * hc.x1 - 2 * wn * sigma * hc.x2 + wn^2 * u
        hc.x1 += dx1 * dt
        hc.x2 += dx2 * dt
        return hc.x1
    end

    timer(hc::HeaterCooler, t::Float64, dt::Float64, params::Params)
    cum_t = 0.0
    current_phase = :steady
    for (phase, dur) in params.phases
        if t < cum_t + dur
            current_phase = phase
            break
        end
        cum_t += dur
    end

    # P = (hc.ṁ * params.cp_oil) * (T_in - T_out)
    # dT = T_in - T_out 

    if current_phase == :charge
        T_set = min(params.dT_max, params.T_hot - T_out)
    elseif current_phase == :discharge
        T_set = max(params.dT_min, -(T_out - params.T_cold))
    else
        T_set = 0
    end
    T_in = heat_cool(params, hc, T_set, dt) + T_out
    return T_in
end

    #=
    dT = (params.T_hot - params.T_cold)/params.tau_heater*dt

    if current_phase == :charge
        if hc.T_set < params.T_hot
            hc.T_set += dT
        end
    elseif current_phase == :discharge
        if hc.T_set > params.T_cold
            hc.T_set -= dT
        end
    else
        hc.T_set = T_out
    end
=#

function log_params(pars::Params)
    @info "R_pipe $(pars.R_pipe) R_solid $(pars.R_solid) length $(pars.L)"
    @info "Nr_solid $(pars.Nr_solid) Nz_solid $(pars.Nz_solid) Nz_fluid $(pars.Nz_fluid)"
    @info "grad $(pars.grad) bump $(pars.bump) cfl $(pars.cfl_target) alpha_f $(pars.alpha_f)"
    @info "T_cold $(pars.T_cold) T_hot $(pars.T_hot) dT_min $(pars.dT_min) dT_max $(pars.dT_max) T_initial $(pars.T_initial)"
    @info "save every $(pars.save_every)"
    @info "$(pars.phases)"
end

"""
Save history data to a file
"""
function save_history(history, filename::String)
    # Convert array of tuples to matrix for easier saving
    if isempty(history)
        println("Warning: History is empty, creating empty file")
        return
    end

    # Convert to matrix (each row is one time step)
    data_matrix = hcat([collect(row) for row in history]...)'

    # Save with header
    open(filename, "w") do file
        writedlm(file, data_matrix, ' ')
    end

    println("History saved to $filename ($(length(history)) time steps)")
end