import numpy as np
import matplotlib.pyplot as plt
import PyQt5 as qt

ptot = 1000 # [MW]
p2max = 400 # [MW]
p23max = 500
t21 = 0.4545
t22 = 0.8182

def C1(x):
    return 30*x + 0.01*x**2

def C2(x):
    return 20*x + 0.02*x**2

def f(x):
    return C1(x[0]) + C2(x[1]) + x[2] * (ptot - x[0] - x[1])

def f2(x):
    return f(x[0:3]) - abs(x[3]) * (p2max - x[1])

def f3(x):
    return f(x[0:3]) - abs(x[3]) * (p23max - t21 * x[0] - t22 * x[1])

def grad(f, x, h=1e-4):
    res = []
    for i in range(len(x)):
        delta = f(x[:i] + [x[i] + h / 2] + x[i+1:]) - f(x[:i] + [x[i] - h / 2] + x[i+1:])
        res += [delta / h]
    return res

def norm(x):
    n = 0
    for d in x:
        n += d**2
    return np.sqrt(n)

def g(x):
    return norm(grad(f, x, h=1e-5))

def g2(x):
    return norm(grad(f2, x, h=1e-6))

def g3(x):
    return norm(grad(f3, x, h=1e-6))

def minimize(f, x0, h=1e-4, step=1e-1, tol=1e-8, N=1e4, echo=False):
    x = x0
    g = grad(f, x, h)
    n = 0
    prev = norm(g) + 2*tol

    while abs(norm(g) - prev) > tol:
        n += 1
        prev = norm(g)
        for i in range(len(x)):
            x[i] -= g[i] * step
        g = grad(f, x, h)

        if (n % 100 == 0) and echo:
            print("Itération ", n)
            print("norm(g) = ", norm(g))
            print("prev = ", prev)
            print("x = ", x)
            print("g = ", g)
        if n > N:
            return x
    return x

def custom_minimize(f, x0):
    res_app = minimize(f, x0, step=5e-1)
    print(res_app)
    res_app = minimize(f, res_app, step=1e-3, tol=1e-12, h=1e-5)
    print(res_app)
    res_app = minimize(f, res_app, step=1e-5, tol=1e-14, h=1e-5)
    print(res_app)
    res_app = minimize(f, res_app, step=1e-6, tol=1e-16, h=5e-6)
    print(res_app)
    return res_app


print(minimize(g, [0, 0, 0]))

custom_minimize(g2, [0, 0, 0, 0.01])

custom_minimize(g3, [0, 0, 0, 0.01])