import matplotlib.pyplot as plt
import numpy as np
import math
y0 = 5           #temp poczatakowa
a = 1.5
T = 8
h = 0.01
y_set = 22           # t*  temp zadane
u_max = 70
u_min = 10
d = 0.2
M = 0.25

def fu(e):
    if (e >= M + d):
        return u_max
    if (e < M + d and e >= -M - d):
        return y_set
    if (e < -M - d):
        return u_min

t = np.arange(0, T, h)
y = np.empty(t.size)
us = np.empty(t.size)
es = np.empty(t.size)

y[0] = y0
us[0] = y_set
es[0] = y_set - y[0]

for i in range(0, t.size-1):
    es[i+1] = y_set - y[i]
    us[i + 1] = fu(es[i+1])
    y[i + 1] = y[i] + h * ((us[i] - y[i])/a)

plt.figure(1)
plt.subplots_adjust(hspace = 0.4)
plt.subplot(311)
plt.xlabel("t")
plt.ylabel("e(t)")
plt.plot(t, es, 'o-', markersize=1)
# plt.hlines(0,0, T,'r','dashed')
#plt.axis([20, 100, -20, 50])


plt.subplot(312)
plt.plot(t, y, 'r-', markersize=1)
plt.xlabel("t")
plt.ylabel("y(t)")
# plt.hlines(d+ M + y_set,0, T,'y' ,'dashed')
# plt.hlines(y_set, 0, T, 'b', 'dashed')
# plt.hlines(y_set - M -d,0, T,'y' ,'dashed')
# plt.axis([0, 100, 120, 200])


plt.subplot(313)
plt.plot(t, us, 'r-', markersize=1)
plt.xlabel("t")
plt.ylabel("u(t)")

plt.show()