import matplotlib.pyplot as plt
import numpy as np

y_target0 = 50
y_target1 = 50
y_target2 = 50


def y_target(t):
    if t < 25:
        return y_target0
    elif t < 50:
        return y_target1
    else:
        return y_target2



def init_arr(x0, shape):
    arr = np.zeros(shape)
    arr[0] = x0
    return arr


def x1_prim(x1, x2, u):
    return -x1 + x2 + u


def x2_prim(x1, x2, x3):
    return x1 -(2 * x2) + x3


def x3_prim(x2, x3):
    return x2 - x3


def error(y, t):
    return y_target(t) - y


def reg(e, e_integral):
    return k_p * e + k_i * e_integral


h = 0.01
t0 = 0
T = 10
x1_0 = 0
x2_0 = 0
x3_0 = 0

k_p = 0.05
k_i = 6


t_arr = np.arange(t0, T, h)
x1_arr = init_arr(x1_0, np.shape(t_arr))
x2_arr = init_arr(x2_0, np.shape(t_arr))
x3_arr = init_arr(x3_0, np.shape(t_arr))
e_arr = init_arr(error(x1_arr[0], t_arr[0]), np.shape(t_arr))
e_sum = e_arr[0] * h
u_arr = init_arr(reg(e_arr[0], e_sum), np.shape(t_arr))


for i in range(t_arr.shape[0] - 1):
    x1_arr[i + 1] = x1_arr[i] + h * x1_prim(x1_arr[i], x2_arr[i], u_arr[i])
    x2_arr[i + 1] = x2_arr[i] + h * x2_prim(x1_arr[i + 1], x2_arr[i], x3_arr[i])
    x3_arr[i + 1] = x3_arr[i] + h * x3_prim(x2_arr[i + 1], x3_arr[i])

    e_arr[i + 1] = error(x1_arr[i + 1], t_arr[i + 1])
    e_sum = e_sum + e_arr[i + 1] * h
    u_arr[i + 1] = reg(e_arr[i + 1], e_sum)


f, axes = plt.subplots(3, sharex='all')

y_target_arr = np.ones(t_arr.shape)

for i in range(y_target_arr.shape[0]):
    y_target_arr[i] = y_target(t_arr[i])


axes[0].plot(t_arr, x1_arr, label='x1', color='orange')
axes[0].plot(t_arr, x2_arr, label='x2', color='green')
axes[0].plot(t_arr, x3_arr, label='x3, y', color='blue')
axes[0].plot(t_arr, y_target_arr, label='target y', color='red')
axes[1].plot(t_arr, e_arr, label='error')
axes[2].plot(t_arr, u_arr, label='U')

for ax in axes:
    ax.grid(True)
    ax.legend(loc='upper right')
    ax.set_xlabel('Czas')
plt.show()