import matplotlib.pyplot as plt
import numpy as np


def createArray(x0, shape):
    arr = np.empty(shape)
    arr[0] = x0
    return arr

def error(yTarget, yCurrent):
    return yTarget - yCurrent


def reg(e, sum):
    global k
    global ki
    return (k * e) + (ki * sum)


h = 0.1
t0 = 0
T = 100
a = -0.025
state = 0
y_target = 50
#y_target2 = 90
#y_target3 = 20
y0 = 30
x0 = 0
k = 0.0
ki = 0.0
random = 0
mi = 2.
w = 0.7

tTab = np.arange(t0, T, h)
xTab = createArray(x0, np.shape(tTab))
yTab = createArray(y0, np.shape(tTab))
targets = np.full_like(tTab, y_target)
eTab = createArray(error(yTab[0], targets[0]), np.shape(tTab))
sumTab = createArray(eTab[0]*h, np.shape(tTab))
uTab = createArray(0., np.shape(tTab))

'''
r = int(targets.shape[0]/3)*2
for j in range(int(targets.shape[0]/3), r):
    targets[j] = y_target2
    targets[r] = y_target3
    r = r + 1
'''

for i in range(1, tTab.shape[0]-1):
    yTab[i] = yTab[i - 1] + h * xTab[i-1]                      # y + h * y'(x = y')
    eTab[i] = targets[i] - yTab[i]
    sumTab[i] = sumTab[i - 1] + (eTab[i] * h)
    uTab[i] = k * eTab[i] + ki * sumTab[i]
    xTab[i] = xTab[i-1] + (h*(uTab[i-1] + (random*np.random.rand(1)) + mi*(1-yTab[i-1]**2)*xTab[i-1] - (w*w*yTab[i-1]))) # x + h*x'



    #print(xTab[i])
    #print(uTab[i])
    print(str(i) + ' ' + str(yTab[i]))
    print(str(yTab[i]) +  ' ' + str(yTab[i-1]))
    #print(str(i) + ' y ' + str(yTab[i]) + ' e '  + str(eTab[i]) + ' x '  + str(xTab[i]))




f, axes = plt.subplots(3, sharex='all')

axes[0].plot(tTab, yTab, label='y', color='blue')
axes[0].plot(tTab, targets, label='y target', color='red')


axes[1].plot(tTab, eTab, label='error')
axes[2].plot(tTab, uTab, label='U')

for ax in axes:
    ax.grid(True)
    ax.legend(loc='upper right')
    ax.set_xlabel('Czas')
plt.show()






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


def createArray(x0, shape):
    arr = np.empty(shape)
    arr[0] = x0
    return arr

def error(yTarget, yCurrent):
    return yTarget - yCurrent


def reg(e, sum):
    global k
    global ki
    return (k * e) + (ki * sum)


h = 0.1
t0 = 0
T = 100
a = -0.025
state = 0
y_target = 50
#y_target2 = 90
#y_target3 = 20
y0 = 0
x0 = 0
k = 0.00005
ki = 0.1
random = 0
b = 4   #0.7
c = 0.3

tTab = np.arange(t0, T, h)
xTab = createArray(x0, np.shape(tTab))
yTab = createArray(y0, np.shape(tTab))
targets = np.full_like(tTab, y_target)
eTab = createArray(error(yTab[0], targets[0]), np.shape(tTab))
sumTab = createArray(eTab[0]*h, np.shape(tTab))
uTab = createArray(0., np.shape(tTab))

'''
#r = int(targets.shape[0]/3)*2
#for j in range(int(targets.shape[0]/3), r):
#    targets[j] = y_target2
#    targets[r] = y_target3
#    r = r + 1
'''

for i in range(1, tTab.shape[0]-1):
    yTab[i] = yTab[i - 1] + h * xTab[i-1]                      # y + h * y'(x = y')
    eTab[i] = targets[i] - yTab[i]
    sumTab[i] = sumTab[i - 1] + (eTab[i] * h)
    uTab[i] = k * eTab[i] + ki * sumTab[i]
    xTab[i] = xTab[i-1] + h*(uTab[i-1] + (random*np.random.rand(1)) - b*xTab[i-1] - c*yTab[i-1]) # x + h*x'



    #print(xTab[i])
    #print(uTab[i])
    #print(str(i) + ' y '  + str(yTab[i]) + ' + '  + str(a * yTab[i - 1] + uTab[i - 1]))




f, axes = plt.subplots(3, sharex='all')

axes[0].plot(tTab, yTab, label='y', color='blue')
axes[0].plot(tTab, targets, label='y target', color='red')


axes[1].plot(tTab, eTab, label='error')
axes[2].plot(tTab, uTab, label='U')

for ax in axes:
    ax.grid(True)
    ax.legend(loc='upper right')
    ax.set_xlabel('Czas')
plt.show()

'''