# %% Import of packages:

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

# %% Def of PI controller function:

def fun_pi(y_sp_k, y_k, u_i_km1, pi_params, dt):
    
    (Kp, Ti, b, u_man, u_max, u_min) = controller_params
    e_k = y_sp_k - y_k  # Control error
    u_p_k = Kp*(b*y_sp_k - y_k)  # P term
    u_i_k = u_i_km1 + (Kp*dt/Ti)*e_k  # PI term
    u_k = u_man + u_p_k + u_i_k  # Total control signal
    u_k = np.clip(u_k, u_min, u_max)  # Limit of control
    
    return (u_i_k, u_k)

# %% 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) 
    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 = 600  # [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]
T_sp = 70  # {oC] Setpoint

# %% PI controller settings:

Kp = 50.0  # W/K]
Ti = 120  # [s]
b = 0.3  # Weight factor of setpoint in P term

u_man = 0  # [W]
u_max = 700  # [W]
u_min = 0  # [W]

controller_params = (Kp, Ti, b, u_man, u_max, u_min)

# %% Preallocation of arrays for plotting etc:

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

# %% Initialization:

T_init = 20.0  # [oC]
T_k = T_init  # [oC]
u_i_km1 = 0  # [W]


# %% 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 timer
    t_k = k*dt

    # Setting input:
    if (t_k >= t_start):
        T_sp_k = T_sp
        
    # PI controller:
    (u_i_k, u_k) = fun_pi(T_sp_k, T_k, u_i_km1,
                          controller_params, dt)

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

    # Updating arrays for plotting:
    t_array[k] = t_k
    T_array[k] = T_k
    T_sp_array[k] = T_sp_k
    u_array[k] = u_k

    # Time shift:
    T_k = T_kp1
    u_i_km1 = u_i_k

    # %% Plotting:
        
    if k > 0:
    
        plt.subplot(2, 1, 1)
        plt.grid(which='both', color = 'k')
        plt.xlim(t_start, t_stop)
        plt.ylim(10, 100)
        plt.xlabel('t [s]')
        plt.ylabel('[oC]')
        plt.legend(labels=('T_sp', 'T', 'T_room'),)
        plt.plot([t_array[k-1], t_array[k]],
                 [T_sp_array[k-1], T_sp_array[k]],'r')
        plt.plot([t_array[k-1], t_array[k]],
                 [T_array[k-1], T_array[k]],'b')
        plt.plot([t_array[k-1], t_array[k]],
                 [T_room_array[k-1],T_room_array[k]],'g')
        
        plt.subplot(2, 1, 2)
        plt.grid(which='both', color = 'k')
        plt.xlim(t_start, t_stop)
        plt.ylim(0, 700)
        plt.xlabel('t [s]')
        plt.ylabel('[W]')
        plt.legend(labels=('Power u = P',))
        plt.plot([t_array[k-1], t_array[k]],
                 [u_array[k-1], u_array[k]],'m')
        
        fig1.canvas.draw()
        fig1.canvas.flush_events()
        plt.show()

    time.sleep(t_sleep_k)
 
    # Automatic adjustment of T_sleep:
 
    toc = time.time()
    t_cycle_meas_k = toc - tic
    error_t_cycle = t_cycle_spec - t_cycle_meas_k

    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('---------------')

    t_sleep_kp1=(t_sleep_k+K_sleep_update*error_t_cycle)
    t_sleep_k = t_sleep_kp1

# plt.savefig('prog_kettle_pi_control_realtime_sim.pdf')