# %% Import of packages:

import matplotlib.pyplot as plt
import numpy as np

# %% Def of PI controller function:

def fun_pi(y_sp_k, y_k, u_i_km1, controller_params, dt):
    
    (Kp, Ti, u_man, u_min, u_max) = controller_params
    e_k = y_sp_k - y_k  # Control error
    u_p_k = Kp*(y_sp_k - y_k)  # P term
    u_i_k = u_i_km1 + (Kp*dt/Ti)*e_k  # PI term
    u_i_k = np.clip(u_i_k, -u_max, u_max)  # Limit ui
    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 feedforward controller:

def fun_feedforward(T_sp_k, dT_sp_dt_k,
                        process_params, T_room, dt):
    (C, G, T_min, T_max) = process_params
    u_f_k = C*dT_sp_dt_k - G*(T_room - T_sp_k)
    
    return u_f_k

# %% Calculation of time-derivative with backward diff:
def fun_time_deriv(y_k, y_km1, dt):

    dy_dt_k = (y_k - y_km1)/dt

    return dy_dt_k
    
# %% Def of process simulator:

def process_sim(T_k, u_k, process_params, T_room, 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 = 1  # [s]
t_start = 0  # [s]
t_stop = 1500  # [s]
N_sim = int((t_stop - t_start)/dt) + 1  # Num time-steps

# %% PI controller settings:

Kp = 50.0  # [W/K]
Ti = 120  # [s]

u_fb_man = 0  # [W]
u_fb_min = -700  # [W]
u_fb_max = 700  # [W]

controller_params = (Kp, Ti, u_fb_man, u_fb_min,u_fb_max)

# %% Limits of total control signal:

u_min = 0
u_max = 700  # [W]

# %% 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)
u_array = np.zeros(N_sim)

# %% Initialization:

T_init = 90.0  # [oC]
T_k = T_init  # [oC]
u_i_km1 = 0  # [W]
T_sp_k = 90
T_sp_km1 = 90

# %% Simulation loop:

for k in range(0, N_sim):

    t_k = k*dt

    # Setting input:
    if (0 <= t_k < 1000): T_room_k = 20
    elif (t_k >= 1000): T_room_k = 30

    # PI controller:
    (u_i_k, u_fb_k) = fun_pi(T_sp_k, T_k, u_i_km1,
                          controller_params, dt)
    
    dT_sp_dt_k = fun_time_deriv(T_sp_k, T_sp_km1, dt)
    
    u_ff_k = fun_feedforward(T_sp_k, dT_sp_dt_k,
                        process_params, T_room_k, dt)
    
    # Total control signal:
    u_k = u_fb_k + 0*u_ff_k
    # u_k = np.clip(u_k, u_min, u_max)  # Limiting u

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

    # Updating arrays for plotting:
    t_array[k] = t_k
    T_array[k] = T_k
    T_room_array[k] = T_room_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
    T_sp_km1 = T_sp_k


# %% Plotting:

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

plt.subplot(3, 1, 1)
plt.plot(t_array, T_sp_array,'r', label='T_sp')
plt.plot(t_array, T_array,'b', label='T')
plt.legend()
# plt.ylim(84, 100)
plt.grid()
plt.xlabel('t [s]')
plt.ylabel('[oC]')

plt.subplot(3, 1, 2)
plt.plot(t_array, T_room_array, 'm', label='T_room')
plt.legend()
# plt.ylim(84, 100)
plt.grid()
plt.xlabel('t [s]')
plt.ylabel('[oC]')

plt.subplot(3, 1, 3)
plt.plot(t_array, u_array, 'g', label='Power u = P')
plt.legend()
plt.ylim(0, 400)
plt.grid()
plt.xlabel('t [s]')
plt.ylabel('[W]')

# plt.savefig('prog_kettle_fb_ff_troom_no_ff.pdf')
# plt.savefig('prog_kettle_fb_ff_troom_yes_ff.pdf')
plt.show()