#%% Imports:

import numpy as np
import matplotlib.pyplot as plt

#%% Model params:

c = 4200 # [J/(kg*K)]
rho = 1000 # [kg/m3]
V = 0.2 # [m3]
G = 1000 # [W/K]
Fv = 0.25e-3  # [m3/s]
T_in = 20  # [deg C]
T_a = 20  # [deg C]

T_min = 0
T_max = 100

#%% Calculation of power giving specified static temp: 

T_static = 25  # [deg C]  Static temp

# From model after T' is set to zero (static value) in the model: 
Pd_0 = - (c*rho*Fv*(T_in-T_static) + G*(T_a-T_static))  # [W]

#%% Sim time settings:

dt = 1  # [s]
t_start = 0  # [s]
t_stop = 6000  # [s]
N_sim = int((t_stop - t_start)/dt) + 1

#%% Preallocation of arrays for storing:

t_array = np.zeros(N_sim)
T_array = np.zeros(N_sim)
T_in_array = np.zeros(N_sim)
T_a_array = np.zeros(N_sim)
Pd_array = np.zeros(N_sim)

#%% Sim loop:

T_k = T_init = 20  # [deg C] Initial temp

for k in range(0, N_sim):

    # State limitation:
    T_k = np.clip(T_k, T_min, T_max)

    t_k = k*dt
    
    Pd_k = Pd_0
    T_in_k = T_in
    T_a_k = T_a
    
    t_array[k] = t_k
    T_array[k] = T_k
    T_in_array[k] = T_in_k
    T_a_array[k] = T_a_k
    Pd_array[k] = Pd_k

    # Time derivative:
    dT_dt_k = (Pd_k + (c*rho*Fv)*(T_in-T_k) + G*(T_a_k-T_k))/(c*rho*V)
    T_kp1 = T_k + dt*dT_dt_k
    
    # Time index shift:
    T_k = T_kp1

# %% Plotting:

plt.close('all')
plt.figure(1)

plt.subplot(2, 1, 1)
plt.plot(t_array, T_array, 'r', label='T')
plt.plot(t_array, T_in_array, 'b', label='T_in')
plt.plot(t_array, T_a_array, 'g', label='T_a')
plt.legend()
plt.grid()
plt.xlabel('t [s]')
plt.ylabel('[deg C]')

plt.subplot(2, 1, 2)
plt.plot(t_array, Pd_array, 'm', label='Pd')
plt.legend()
plt.grid()
plt.xlabel('t [s]')
plt.ylabel('[W]')

plt.savefig('plot_sim_heated_tank_2.pdf')
plt.show()