import random

import matplotlib.pyplot as plt
import numpy as np


def error(y):
    return Y_expected - y


a = -0.02
b = 0.7
c = 0.3
T = 100
h = 0.1
Y_expected = 50
Y_0 = 0
X_0 = 0
iterations = (T / h).__int__()
hysteresis = 5
K_P = 0.1
K_I = 0.1

X_results = np.zeros(shape=iterations)
Y_results = np.zeros(shape=iterations)
Y_exp_results = np.zeros(shape=iterations)
U_results = np.zeros(shape=iterations)
E_results = np.zeros(shape=iterations)
E_sum_results = np.zeros(shape=iterations)
T_results = np.zeros(shape=iterations)

X_results[0] = X_0
Y_results[0] = Y_0
E_results[0] = error(Y_0)
E_sum_results[0] = error(Y_0)
T_results[0] = 0
U_results[0] = 0

r = 0

for itr in range(0, iterations - 1):

    print("U")
    print(U_results[itr])
    print("AKTUALNE X")
    print(X_results[itr])
    print("NASTEPNE X")
    print(X_results[itr] + (a * X_results[itr] + U_results[itr]) * h)
    print("Y")
    print( Y_results[itr])
    print('')


    U_results[itr+1] = K_P * error(Y_results[itr]) +  K_I * E_sum_results[itr]

    Y_exp_results[itr] = Y_expected

    X_results[itr + 1] = X_results[itr] + (U_results[itr]+r*np.random.randn(1)-b*X_results[itr]-c*Y_results[itr]) * h

    Y_results[itr + 1] = Y_results[itr] + (X_results[itr]) * h

    E_results[itr + 1] = error(Y_results[itr])

    E_sum_results[itr+1] = E_sum_results[itr] + E_results[itr]*h

    T_results[itr + 1] = T_results[itr] + h

plt.figure()
plt.plot(T_results, X_results)
plt.title("X to T")
plt.show()

plt.figure()
plt.plot(T_results, Y_results)
plt.title("Y to T")
plt.show()

plt.figure()
plt.subplot(3, 1, 1)
plt.plot(T_results, Y_results, label='y')
plt.plot(T_results,Y_exp_results, label='y expected', color='red')
#plt.plot(T_results, np.ones(T_results.shape) * Y_expected, label='y expected', color='red')
# plt.axis([1,5,0,100])
plt.legend()
plt.grid(True)
plt.title("Y to T")

plt.subplot(3, 1, 2)
plt.plot(T_results, E_results)
# plt.axis([1,5,0,-50])
plt.title("E to T")

plt.subplot(3, 1, 3)
plt.plot(T_results, U_results)
# plt.axis([1,5,0,50])
plt.grid(True, color='red')
plt.title("U to T")
plt.show()