import numpy as np
import matplotlib.pyplot as plt
import scipy.optimize as scp



def myFunc(K):
    T = 30
    h = 0.01
    y = 0
    x = 1
    xThres = 0
    error=0

    a = 0.7
    b = 0.7
    c = 3
    z = -0.4
    columns = (T // h) + 1
    table = np.zeros(shape=(3, int(columns)))
    tableU = np.zeros(shape=(2, int(columns)))
    tableE = np.zeros(shape=(2, int(columns)))
    table[0][0] = y
    table[1][0] = x
    tableE[0][0] = xThres - x
    e_sum = tableE[0, 0] * h
    tableU[0, 0] = K[0] * tableE[0, 0] + K[1] * e_sum
    if tableU[0, 0] > 2:
        tableU[0, 0] = 2
    elif tableU[0, 0] < -2:
        tableU[0, 0] = -2
    nextX = x
    nextY = y

    for j in range(1, int(columns)):
        table[0, j] = nextY + (-(1 / c) * (nextX - a + b * nextY)) * h
        nextY = table[0, j]
        table[1, j] = nextX + (c * (nextY - ((nextX * nextX * nextX) / 3) + nextX + z) + tableU[0, j - 1]) * h
        nextX = table[1, j]
        table[2, j] = table[2, j - 1] + h
        e_pop = tableE[0, j - 1]
        tableE[0, j] = xThres - nextX
        e_sum += tableE[0, j] * h
        tableU[0, j] = K[0] * tableE[0, j] + K[1] * e_sum + K[2] * ((-e_pop + tableE[0, j]) / h)
        if tableU[0, j] > 2:
            tableU[0, j] = 2
        elif tableU[0, j] < -2:
            tableU[0, j] = -2
        tableU[1, j] = tableU[1, j - 1] + h
        tableE[1, j] = tableE[1, j - 1] + h
        error += abs(tableE[0, j]) * tableE[1, j]
    return error


print(scp.fmin_bfgs(myFunc,[1,1,1]))