import matplotlib.pyplot as plt
import numpy as np


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


def yPrim(y, u):
    return a * y + u


def error(y):
    return y_target - y


def reg(e):
    global state
    if e > maxError:
        state = True
    if e < -maxError:
        state = False
    return u_on if state else u_off


h = 0.01
t0 = 0
T = 100
a = -0.025
u_on = 2
u_off = 0
state = False
maxError = 5
y_target = 50
y0 = 75

tTab = np.arange(t0, T, h)
yTab = createArray(y0, np.shape(tTab))
eTab = createArray(error(yTab[0]), np.shape(tTab))
uTab = createArray(reg(eTab[0]), np.shape(tTab))

for i in range(tTab.shape[0] - 1):
    yTab[i + 1] = yTab[i] + h * yPrim(yTab[i], uTab[i])
    eTab[i + 1] = error(yTab[i + 1])
    uTab[i + 1] = reg(eTab[i + 1])


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

axes[0].plot(tTab, yTab, label='y', color='orange')
axes[0].plot(tTab, np.ones(tTab.shape)*y_target, label='y target', color='red')
axes[1].plot(tTab, eTab, label='error')
axes[2].plot(tTab, uTab, label='U')

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