import matplotlib.pyplot as plt
import numpy as np

X_0=93
a=-0.5
T=40
h=0.1
Y_expected=50
Y_0=100
iterations=(T/h).__int__()
hysteresis = 5

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

X_results[0]=X_0
Y_results[0]=Y_0
T_results[0]=0


U_prev = 0

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

    error = Y_expected - Y_results[itr]

    U = U_prev

    if (error > 0 and Y_results[itr] < Y_expected - hysteresis):
        U = 30
    elif(error <= 0 and Y_results[itr] > Y_expected + hysteresis):
        U = 0

    U_results[itr] = U

    U_prev = U

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

    print("Y: ")
    print(Y_results[itr])
    print("Error: ")
    print(error)
    print('')

    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(2,1,1)
plt.plot(T_results,Y_results)
plt.axis([2,8,40,100])
plt.grid(True)
plt.title("Y to T")
plt.subplot(2,1,2)
plt.plot(T_results,U_results,'bo')
plt.axis([2,8,0,100])
plt.grid(True,color='red')
plt.title("U to T")
plt.show()