import matplotlib.pyplot as plt
import numpy as np
import math
import random
T = 30
h = 0.01
size = int(T / h)

kp = 5  # im większe tym dokładniej pracuje układ regulacji
ki = 2  #czym wieksze tym czestsze falki
y_set = 50
y_0 = 40
a = -0.05


y = np.zeros(size)
e = np.zeros(size)
u = np.zeros(size)
t = np.zeros(size)
y_need_arr = np.array(size)
x1 = np.zeros(size)
x2 = np.zeros(size)
x3 = np.zeros(size)

# y[0] = y_0
e[0] = y_set - y[0]
u[0] = kp*e[0]
e_sum = e[0]*h
x1[0] = 4
x2[0] = 7
x3[0] = 5
y[0] = x3[0]

for i in range(size - 1):

    # x[i+1] = x[i] + h * (y[i])
    # y[i+1] = y[i] + h * (-a * x[i] - b * y[i] + u[i] + r * np.random.rand())

    x1[i+1] = x1[i] + h * (-x1[i] + x2[i] + u[i])
    x2[i+1] = x2[i] + h *(x1[i] -2*x2[i] + x3[i])
    x3[i + 1] = x3[i] + h * (x2[i] - 2*y[i])
    y[i+1] = x3[i+1]

    # y[i + 1] = y[i] + h * (a * y[i] + u[i])
    e[i + 1] = y_set - y[i + 1]
    e_sum = e_sum + e[i + 1] * h
    u[i+1] = kp*e[i+1] + ki *e_sum
    t[i + 1] = t[i] + h

    # if(i == 3000):
    #     y_set = 80

# i = t/h
# print(random.uniform(0, 1))

plt.subplot(311)
# plt.hlines(y_set, 0, T, 'b', 'dashed')
# plt.hlines(50, 0, 30, 'b', 'dashed')
# plt.plot(t[3000:], np.ones(3000) * y_set, 'b', markersize=1)
# plt.plot(t[3000:], np.ones(size-3000) * 80, 'b', label='y*', markersize=1)
# plt.plot(t[:3000], np.ones(3000) * 50, 'b', label='y*', markersize=1)
# plt.plot(t[:3000], np.ones(3000) * y_set, 'b', label='y*', markersize=1)
# plt.hlines(80, 30, T, 'b', 'dashed')
plt.plot(t, x3, 'r', label='y', markersize=1)
plt.plot(t, x1, 'b', label='y', markersize=1)
plt.plot(t, x2, 'y', label='y', markersize=1)
# plt.legend(bbox_to_anchor=(1.005, 1), loc=2, borderaxespad=0.)
plt.ylabel('y(t)')
plt.xlabel('t')


plt.subplot(312)
plt.plot(t, e, 'r', markersize=1)
plt.hlines(0,0, T,'b','dashed')
plt.ylabel('e(t)')
plt.xlabel('t')

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

plt.show()