import numpy as np
import matplotlib.pyplot as plt
import random

a = 1.5
u = 85
e_upper_value = 4
e_lower_value = -2
y_min = 10
y_set = 22
y_max = 70

t0 = 0
t_max = 150
h = 0.01

t = np.arange(0, t_max, h)
y = np.zeros(len(t))
y[0] = 22
u_results = np.zeros(len(t))
e_results = np.zeros(len(t))
e = y_set - y[0]

for i in range(0, len(t)-1):
    e = y_set - y[i]
    if e > e_upper_value:
        u = 85
    else:
        if e < e_lower_value:
            u = 0

    y[i+1] = y[i] + h*(a*y[i]+u)
    u_results[i] = u
    e_results[i] = e


def bez_histerezy():
    t = np.arange(0, t_max, h)
    y = np.zeros(len(t))
    y[0] = 0
    u_results = np.zeros(len(t))
    e_results = np.zeros(len(t))
    e = y_set - y[0]

    for i in range(0, len(t)-1):
        e = y_set - y[i]
        if e > 0:
            u = y_max
        else:
            u = y_min

        if y[i] + h*(u-y[i])/a < y_min:
            y[i+1] = y_min
        elif y[i] + h*(u-y[i])/a > y_max:
            y[i+1] = y_max
        else:
            y[i+1] = y[i] + h*(u-y[i])/a
        u_results[i] = u
        e_results[i] = e
    return y

def dwupolozeniowy_z(d):
    t = np.arange(0, t_max, h)
    y = np.zeros(len(t))
    y[0] = 0
    u_results = np.zeros(len(t))
    e_results = np.zeros(len(t))
    e = y_set - y[0]

    for i in range(0, len(t)-1):
        e = y_set - y[i]
        if e > d:
            u = y_max
        elif e < -d:
            u = y_min

        if y[i] + h*(u-y[i])/a < y_min:
            y[i+1] = y_min
        elif y[i] + h*(u-y[i])/a > y_max:
            y[i+1] = y_max
        else:
            y[i+1] = y[i] + h*(u-y[i])/a
        u_results[i] = u
        e_results[i] = e
    return y

def trojpolozeniowy_bez(M):
    t = np.arange(0, t_max, h)
    y = np.zeros(len(t))
    y[0] = 0
    u_results = np.zeros(len(t))
    e_results = np.zeros(len(t))
    e = y_set - y[0]

    for i in range(0, len(t)-1):
        e = y_set - y[i]
        if e >= M:
            u = y_max
        elif e < -M:
            u = y_min
        else:
            u = y_set

        if y[i] + h*(u-y[i])/a < y_min:
            y[i+1] = y_min
        elif y[i] + h*(u-y[i])/a > y_max:
            y[i+1] = y_max
        else:
            y[i+1] = y[i] + h*(u-y[i])/a
        u_results[i] = u
        e_results[i] = e
    return y

def trojpolozeniowy_z(M, d):
    t = np.arange(0, t_max, h)
    y = np.zeros(len(t))
    y[0] = 0
    u_results = np.zeros(len(t))
    e_results = np.zeros(len(t))
    e = y_set - y[0]

    for i in range(0, len(t)-1):
        e = y_set - y[i]
        if e > d:
            if e >= M:
                u = y_max
            else:
                u = y_set
        elif e < -d:
            if e < -M:
                u = y_set
            else:
                u = y_min

        if y[i] + h*(u-y[i])/a < y_min:
            y[i+1] = y_min
        elif y[i] + h*(u-y[i])/a > y_max:
            y[i+1] = y_max
        else:
            y[i+1] = y[i] + h*(u-y[i])/a
        u_results[i] = u
        e_results[i] = e
    return y


if __name__ == '__main__':
    #bez_histerezy(t)
    plt.subplot(411)
    plt.plot(t, bez_histerezy())
    plt.xlabel('t')
    plt.ylabel('y(t)')
    plt.plot(t, np.full(t.shape, [y_set]))
    plt.xlim(0, 10)
    plt.ylim(0, 30)

    plt.subplot(412)
    plt.plot(t, dwupolozeniowy_z(5))
    plt.xlabel('t')
    plt.ylabel('y(t)')
    plt.plot(t, np.full(t.shape, [y_set]))
    plt.xlim(0, 10)
    plt.ylim(0, 30)

    plt.subplot(413)
    plt.plot(t, trojpolozeniowy_bez(0.2))
    plt.xlabel('t')
    plt.ylabel('y(t)')
    plt.plot(t, np.full(t.shape, [y_set]))
    plt.xlim(0, 150)
    plt.ylim(0, 30)

    plt.subplot(414)
    plt.plot(t, trojpolozeniowy_z(0.2, 0.3))
    plt.xlabel('t')
    plt.ylabel('y(t)')
    plt.plot(t, np.full(t.shape, [y_set]))
    plt.xlim(0, 10)
    plt.ylim(0, 40)

    # plt.plot(t, bez_histerezy())
    # plt.xlabel('t')
    # plt.ylabel('y(t)')
    # plt.plot(t, np.full(t.shape, [y_set]))
    # plt.xlim(0, 10)
    # plt.ylim(10, 70)
    #
    # #e(t)
    # plt.subplot(312)
    # plt.plot(t, e_results)
    # plt.xlabel('t')
    # plt.ylabel('e(t)')
    # plt.xlim(2, 4)
    # plt.ylim(-5, 20)
    #
    # #u(t)
    # plt.subplot(313)
    # plt.plot(t, u_results)
    # plt.xlabel('t')
    # plt.ylabel('u(t)')
    # plt.xlim(2, 4)
    # plt.ylim(0, 90)
    # y_t.plot(t[220:350], y[220:350])
    # y_t.plot(t[220:350], np.full([130], y_set), 'r')
    # y_t.plot(t[220:350], np.full([130], y_set-e_lower_value), 'r--')
    # y_t.plot(t[220:350], np.full([130], y_set-e_upper_value), 'r--')
    # y_t.set(xlabel = 't', ylabel = 'y(t)')
    # e_t = figure.add_subplot(312)
    # e_t.plot(t[220:350], e_results[220:350])
    # e_t.set(xlabel = 't', ylabel = 'e(t)')
    # u_t = figure.add_subplot(313)
    # u_t.plot(t[220:350], u_resutls[220:350], 'r.')
    # u_t.set(xlabel = 't', ylabel = 'u(t)')
    plt.show()