# %% Import of packages:

import matplotlib.pyplot as plt
import numpy as np
import time

# %% Def of process model simulator

def process_sim(T_k, u_k, process_params, dt):

    (C, G, T_min, T_max) = process_params
    T_k = np.clip(T_k, T_min, T_max)  # State lim
    dT_dt_k = (1/C)*(u_k + G*(T_room - T_k)) #Time deriv.
    T_kp1 = T_k + dt*dT_dt_k  # Euler integration

    return T_kp1

# %% Model parameters:

H = 0.079  # [m]
D = 0.090  # [m]
c = 4180  # [J/(kg*K)]
rho = 1000  # [kg/m3]
k_tc = 0.2  # [(W*m/(m2*K)) = W/(m*K)]
L = 3e-3  # [m]

# Derived model params:

V = H*np.pi*(D/2)**2  # [m3]
C = c*rho*V  # [J/K]
A = np.pi*D*H + 2*np.pi*(D/2)**2  # [m2]
G = (k_tc/L)*A  # [W/K]

T_min = 0  # [oC] State limit
T_max = 100  # [oC] State limit

process_params = (C, G, T_min, T_max)

# %% Simulation time settings:

dt = 10  # [s]
t_start = 0  # [s]
t_stop = 400  # [s]
N_sim = int((t_stop - t_start)/dt) + 1
K_sim_speed = 10
t_cycle_spec = dt/K_sim_speed
K_sleep_update = 0.1  
t_sleep_k = 1  # Initial time used in time.sleep()

# %% Params of input signals:

T_room = 20  # [oC]

# %% Preallocation of arrays for plotting etc:

t_array = np.zeros(N_sim)
T_array = np.zeros(N_sim)
T_room_array = np.zeros(N_sim) + T_room
P_array = np.zeros(N_sim)

# %% Initialization:

T_k = T_init = 20.0  # [oC]

# %% Preparing for plotting:

plt.close('all')
fig1 = plt.figure(1, figsize=(12, 9))

# %% Simulation loop:

for k in range(0, N_sim):

    tic = time.time()  # Starting the timer
    t_k = k*dt

    if (0 <= t_k <= 10): P_k = 0
    else: P_k = 700

    # Process sim (Euler integration):
    T_kp1 = process_sim(T_k, P_k, process_params, dt)

    # Updating arrays for plotting:
    t_array[k] = t_k
    T_array[k] = T_k
    P_array[k] = P_k

    # Time shift:
    T_k = T_kp1

    # %% Plotting (continuously updated):
        
    if k > 0:
    
        plt.subplot(2, 1, 1)
        plt.plot([t_array[k-1], t_array[k]],
                 [T_array[k-1], T_array[k]],'b',
                 label='T')
        plt.plot([t_array[k-1], t_array[k]],
                 [T_room_array[k-1],T_room_array[k]],'g',
                 label='T_room')
        plt.legend()
        plt.grid(which='both', color = 'grey')
        plt.xlim(t_start, t_stop)
        plt.ylim(10, 110)
        plt.xlabel('t [s]')
        plt.ylabel('[oC]')
        
        plt.subplot(2, 1, 2)
        plt.plot([t_array[k-1], t_array[k]],
                 [P_array[k-1], P_array[k]],'r',
                 label='P')
        plt.legend()
        plt.grid(which='both', color = 'grey')
        plt.xlim(t_start, t_stop)
        plt.ylim(-100, 800)
        plt.xlabel('t [s]')
        plt.ylabel('[W]')
        
        fig1.canvas.draw()
        fig1.canvas.flush_events()
        plt.show()

    time.sleep(t_sleep_k)
 
    # %% Automatic adjustment of t_sleep:
 
    toc = time.time()  # Stopping the timer
    t_cycle_meas_k = toc - tic
    error_t_cycle = t_cycle_spec - t_cycle_meas_k
    t_sleep_kp1=(t_sleep_k+K_sleep_update*error_t_cycle)
    t_sleep_k = t_sleep_kp1
    
    # %% Printing time params:

    print('---------------')
    print('dt =', f'{dt:.3f}')
    print('t_cycle_spec =', f'{t_cycle_spec:.3f}')
    print('t_cycle_meas =', f'{t_cycle_meas_k:.3f}')
    print('error_t_cycle =', f'{error_t_cycle:.3f}')
    print('t_sleep =', f'{t_sleep_k:.3f}')
    print('---------------')


# plt.savefig('prog_kettle_realtime_sim.pdf')