import matplotlib.pyplot as plt
import numpy as np

# Model parameters:

Tf = 1.0  # [s]

# %% Simulation time settings:

dt = 0.01  # [s]
t_start = 0  # [s]
t_stop = 5  # [s]
N_sim = int((t_stop - t_start)/dt) + 1  # Num time-steps

# %% Preallocation of arrays for plotting:

t_array = np.zeros(N_sim)
y_i_array = np.zeros(N_sim)
y_u_array = np.zeros(N_sim)

# %% Params of input signals:

t0 = 2.0  # [s]
Y_i_0 = 0
Y_i_1 = 1

# %% Initialization:

y_u_init = 0.5

# %% Preallocation of array for time-delay:

Nf = int(round(Tf/dt)) + 1
delay_array = np.zeros(Nf) + y_u_init

# %% Simulation loop:

for k in range(0, N_sim):

    t_k = k*dt

    # Selecting inputs:
    if (t_k >= t_start and t_k < t0):
        y_i_k = Y_i_0
    elif (t_k >= t0):
        y_i_k = Y_i_1

    # Moving array elements one step:
    y_u_k = delay_array[-1]
    delay_array[1:] = delay_array[0:-1]
    delay_array[0] = y_i_k

    # Arrays for plotting:
    t_array[k] = t_k
    y_i_array[k] = y_i_k
    y_u_array[k] = y_u_k

# %% Plotting:

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

plt.plot(t_array, y_i_array, 'r', label='Input y_i')
plt.plot(t_array, y_u_array, 'b', label='Output y_u')
plt.legend()
plt.grid()
plt.xlabel('t [s]')

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