# %% Import of packages:

import matplotlib.pyplot as plt
import numpy as np

# %% Def of on/off controller:

def fun_on_off(y_sp_k, y_k, controller_params,
               contr_state):
    
    # Control error:
    e_k = y_sp_k - y_k
    
    # Setting state of the controller:
    if e_k <= e_min:
        contr_state = False
    if e_k >= e_max:
        contr_state = True
    if (e_min <= e_k <= e_max) and (contr_state == True):
        contr_state = True
    if (e_min <= e_k <= e_max) and (contr_state== False):
        contr_state = False

    # Selecting control signal based on controller state:
    if contr_state == True:
        u_k = u_on
    if contr_state == False:
        u_k = u_off
     
    return (u_k, contr_state)

# %% 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 parameters:

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 = 0.1  # [s]
t_start = 0  # [s]
t_stop = 1000  # [s]
N_sim = int((t_stop - t_start)/dt) + 1  # Num time-steps

# %% Params of input signals:

T_room = 20  # [oC]
T_sp = 70  # [oC] Setpoint

# %% Controller params:

u_on = 700  # [W]
u_off = 0  # [W]
e_max = 5  # [oC]
e_min = -5  # [oC]

controller_params = (u_on, u_off, e_max, e_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)

# %% State limits:

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

# %% Initialization:

T_init = 20.0  # [oC]
T_k = T_init  # [oC]
contr_state = True

# %% Simulation loop:

for k in range(0, N_sim):

    t_k = k*dt

    # Setting input:
    if (t_k >= t_start):
        T_sp_k = T_sp
        
    # On/off controller:
    (u_k, contr_state) = fun_on_off(T_sp_k, T_k,
                                  controller_params,
                                  contr_state)

    # 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

# %% Plotting:

plt.close('all')
plt.figure(num=1, figsize=(12, 9))
plt.suptitle('Sim of kettle')

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

plt.subplot(2, 1, 1)
plt.plot(t_array, T_sp_array,'r', label='T_sp')
plt.plot(t_array, T_array,'b', label='T')
plt.plot(t_array, T_room_array,'g', label='T_room')
plt.legend()
plt.xlabel('t [s]')
plt.ylabel('[oC]')
plt.grid()

plt.subplot(2, 1, 2)
plt.plot(t_array, u_array, 'm', label='Power u = P')
plt.legend()
plt.grid()
plt.xlabel('t [s]')
plt.ylabel('[W]')

# plt.savefig('prog_kettle_on_off_control.pdf')
plt.show()