import matplotlib.pyplot as plt
import numpy as np


def przelacznik2polozeniowy(e):
    if(e>0):
        return 22
    return 0

def przelacznik2polozeniowyZHistereza(e, uPoprzednie):
    if(e>1):
        return 30
    elif (e<-1):
        return 0
    return uPoprzednie

def przelacznik3polozeniowy(e, uPoprzednie):
    if(e>0):
        return 17
    elif (e<0):
        return -17
    return 0

def przelacznik3polozeniowyZHistereza(e, uPoprzednie, h):
    if(e>h):
        return 17
    elif (e<-h):
        return -17
    elif((e>0 and uPoprzednie>0) or (e<0 and uPoprzednie<0)):
        return uPoprzednie
    return 0


zadana = 5

T = 10
deltaT = 0.1
t = np.arange(0., T, deltaT)

y = np.empty_like(t)
y[0] = 10

e = np.empty_like(t)
e[0]=zadana-y[0]

e_kw=np.empty_like(t)
e_kw[0]=e[0]*e[0]*deltaT
sum=e_kw[0]

u = np.empty_like(t)
u[0] = przelacznik3polozeniowy(e[0],0)

for h in range(1,5):
    for i in range(len(t) - 1):
        y[i + 1] = y[i] + deltaT * (-3 * y[i] + u[i]-1)
        e[i+1]=zadana-y[i+1]
        sum+=((zadana-y[i+1])*(zadana-y[i+1]))*deltaT
        e_kw[i+1] = sum
        u[i+1]=przelacznik3polozeniowyZHistereza(e[i+1],u[i],h/10)
    plt.subplot(211)
    plt.plot(e_kw,t)
    plt.subplot(212)
    plt.plot(t,e)
plt.show()


# plt.subplot(311)
# plt.plot(t,e)
# plt.axhline(0,color="k")
# plt.subplot(312)
# plt.plot(t,u)
# plt.plot(t,e_kw)
# plt.show()